#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <fstream>

#include "std_msgs/msg/float32_multi_array.h"
#include "geometry_msgs/msg/pose_with_covariance_stamped.h"
#include <tf2/transform_datatypes.h>
#include <tf2/convert.h>
#include "ubt_interface/srv/odom_reset.hpp"
#include "ubt_com/ubt_com_util.h"

#include "ubt_interface/srv/bel_pose.hpp"
#ifdef UBT_USLAM_SUPPORT
    #include "relocalization_module_pkg/EstimateRelocalization.h"
    #include "maps_manager/SetSlamMap.h"
#endif
#include "ubt_state/ubt_state_util.h"
#include "ubt_state/ubt_state_task_map.h"
#include "ubt_state/ubt_state_task_ex.h"
#include "ubt_state/ubt_state_task_adjust.h"
#include "ubt_state/node.h"
#include "ubt_state/ubt_state_diagram.h"
#include "unistd.h"

namespace ubt_state
{
CStateDiagram::CStateDiagram()
{
    _curState = ubt_diagram_initial;    /* Shiny : 开机时候不上报close状态 */
    _curStateEnterTime = ubt_com::GetNowInMsecs();
    _curTask = NULL;
    _curTaskId = "";
    _curTaskFlag = 0;
    _lastState = ubt_diagram_closed;
    _statePub = ubt_Initial;    /* Shiny : 开机时候不上报close状态 */
    _statePubEnterTime = _curStateEnterTime;
    memset(&_curStatus, 0, sizeof(_curStatus));
    _curStatus.liftState = lift_state_unknown;
    _curStatus.dragState = drag_state_unknown;
    _curStatus.autoFindState = false;
    _curStatus.isStandaloneMode = false;
    _bStopTrigged = false;
    _initPoseBelCnt = 0;
    _initPoseSel = -1;
    _initPoseBel = 0.0f;
    _initPose.x = _initPose.y = _initPose.theta = 0.0f;

    _avoid_laser_state_stime = 0;
    _avoid_rgbd_state_stime = 0;
    _obstacle_alarm_cnt = 0;
    _rmctlMode = 0;
    _screen_pub_mode = 0;
    _poseChecker.state = 0;
    _isRefault = false;
    _is_characteristic = 0;
    _is_initial = false;
    _auto_find_obs = false;
    _pub_InitialPose = true;
    _characteristic_rfid = "";
    _is_finish_current_task = true;
    _is_update = false;
    _event_emergency_msg = -1;
    _autoFindPauseFlag = false;
    _last_ble_con_state.data = 0;
}

CStateDiagram::~CStateDiagram()
{
}

#define UBT_STATE_VERSION "3.6.602"
int CStateDiagram::Run(int argc, char *argv[])
{
    
    for (int i = 1; i < argc; i++)
    {
        if (!strcmp(argv[i], "-v"))
        {
            printf("%s version=%s\n", argv[0], UBT_STATE_VERSION);
            return 0;
        }
    }
    rclcpp::sleep_for(std::chrono::milliseconds(2000));

    RCLCPP_INFO(node->get_logger(),"argv= %s version=%s start", argv[0], UBT_STATE_VERSION);
    if (0 != _LoadCfg())
    {
        return -1;
    }

    _curStatus.expLength = _cfg.robot_length;
    _curStatus.expWidth = _cfg.robot_width;
    RCLCPP_INFO(node->get_logger(),"load cfg done ,state_save_file=%s obstacle_alarm_duration=%d obstacle_dis=(%f  %f %f %f ) obstacle_map_dis=%f obstacle_rmctl_dis=%f first_locate_bel_threshold=%f pose_fail_bel_threshold=%f last_pose='%s'  _initPoses.size()=%d init_poses='%s' pose_check_dis=%f pose_check_ratio=%f",
             _cfg.state_save_file.c_str(), _cfg.obstacle_alarm_duration,
             _cfg.obsParams.stopDis, _cfg.obsParams.slowDis, _cfg.obsParams.warnDis, _cfg.obsParams.sideDis, _cfg.obstacle_mapping_dis, _cfg.obstacle_rmctl_dis,
             _cfg.first_locate_bel_threshold,
             _cfg.pose_fail_bel_threshold,
             _cfg.last_pose.c_str(), (int)_initPoses.size(), _cfg.init_poses.c_str(),
             _cfg.pose_check_dis, _cfg.pose_check_ratio);
    UbtStateUtilInit();
    _SetObsParams(0);
    CTaskEx::ClassInit(_cfg.obsParams);
    _InitForROS();
    
    rclcpp::spin(node); 
 

    CTaskEx::ClassCleanup();
    UbtStateUtilCleanup();
    _SaveCfg();
    // 关闭ROS2 C++接口
    rclcpp::shutdown();  
    
    return 0;
}

StateStatus *CStateDiagram::GetStateStatus()
{
    return &_curStatus;
}

void CStateDiagram::NotifyTaskEvent(std::string taskEvent)
{
    RCLCPP_INFO(node->get_logger(),"taskEvent= %s", taskEvent.c_str());
    std_msgs::msg::String msg;
#if 0
    if (msg.data == UBT_TASK_EVENT_NAV_POSEJUMP_STR)
    {
        //位姿跳变，定位失败
        _SetPoseState(pose_state_waitloc);
    }
#endif
    msg.data = taskEvent.c_str();

}
//验证任务状态和阶段
void CStateDiagram::NotifyTaskStateAndStage(int state, int stage, int stateReason, std::string stateReasonDesc)
{
    if (_curTask == NULL)
    {
        RCLCPP_WARN(node->get_logger(),"[CStateDiagram] NotifyTaskStateAndStage : curTask is null!!!");
        return;
    }
    RCLCPP_INFO(node->get_logger(),"[CStateDiagram] NotifyTaskStateAndStage : state=%d stage=%d stateReason=%d", state, stage, stateReason);

    ubt_state::ulogger->info("[CStateDiagram] NotifyTaskStateAndStage : state={} stage={} stateReason={}", state, stage, stateReason);

    int index, reportIndex, reportArrivedEvent = 0;
    _curTask->GetIndex(index, reportIndex);
    std::string actionType = _curTask->GetActionType();
    std::string status;

    ubt_task_state state2 = _curTask->GetState();

    ubt_state::ulogger->info("[CStateDiagram] NotifyTaskStateAndStage : state2={} stage={} stateReason={}", state2, stage, stateReason);
    
    char buf[256];
    buf[sizeof(buf) - 1] = 0;
    if (state2 == ubt_task_state_closed)
    {
        int stopReason;
        std::string stopReasonDesc;
        _curTask->GetStopInfo(stopReason, stopReasonDesc);

        ubt_state::ulogger->info("[CStateDiagram] NotifyTaskStateAndStage : stopReason: {}", stopReason);

        snprintf(buf, sizeof(buf) - 1, "stopReason=%d&stopReasonDesc=%s", stopReason, stopReasonDesc.c_str());
        if (stopReason == ubt_task_result_ok)
        {
            if (_needArrived)
            {
                reportArrivedEvent = 1;
            }
            status = UBT_TASK_STATUS_FINISH_STR;
        }
		else if (stopReason == ubt_task_result_usr_cancel ||
            stopReason == ubt_task_result_usr_dispatchcancel ||
            stopReason == ubt_task_result_navigation_cancel ||
            stopReason == ubt_task_result_action_cancel)
		{
			status = UBT_TASK_STATUS_WARNING_STR;
		}
        else
        {
            status = UBT_TASK_STATUS_ERROR_STR;
        }
    }
    else
    {
        if ((_needArrived) && (reportIndex > 1))
        {
            reportArrivedEvent = 1;
        }
        status = UBT_TASK_STATUS_INPROCESS_STR;
        snprintf(buf, sizeof(buf) - 1, "state=%d&stage=%d", state2, _curTask->GetStage());
    }

    if (reportArrivedEvent == 1)
    {
        _needArrived = false;
        _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ARRIVED_STR, 1, "nav", "");
    }

    std::string desc = std::string(buf);
    _NotifyTaskStatus(_curTaskId, status, reportIndex, actionType, desc);

    if (state == ubt_task_state_closed)
    {
        if (_curTask != NULL)
        {
            RCLCPP_INFO(node->get_logger(),"CStateDiagram::NotifyTaskStateAndStage : delete current task: %d", _curTask->GetStage());
            delete _curTask;
            _curTask = NULL;
            _curTaskId = "";
            _needArrived = false;
            _curTaskFlag = 0;
        }
        if (_curState == ubt_diagram_run)
        {
            if (_bStopTrigged)
            {
                _SetState(ubt_diagram_initial);
            }
            else
            {
                _SetState(ubt_diagram_wait);
            }
        }
    }
    _RefreshState();
}
//启停建图任务
void CStateDiagram::Lift_Speed_And_Distance_Screen_Callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
{
    if (msg->data[1] > 0)
    {
        _screen_pub_mode = 2;
    }
    else
    {
        _screen_pub_mode = 1;
    }
}

void CStateDiagram::_PublishScreenMode()
{
    RCLCPP_INFO(node->get_logger(),"Current screen mode is : %d", _screen_pub_mode);
    if (_screen_pub_mode == 0)
    {
        return;
    }
    
    std_msgs::msg::Float32MultiArray liftTypeAndHeight;
    if (_screen_pub_mode == 2)
    {
        liftTypeAndHeight.data.push_back(0.08);
        liftTypeAndHeight.data.push_back(90.0);
        if (_curStatus.liftState != lift_state_up)
        {
            RCLCPP_INFO(node->get_logger(),"publishing fork up topic and lift_state is : %d", _curStatus.liftState);
            _pub_lift_speed_and_distance->publish(liftTypeAndHeight);
        }
        else
        {
            _screen_pub_mode = 0;
        }
    }
    else if (_screen_pub_mode == 1)
    {
        liftTypeAndHeight.data.push_back(-0.08);
        liftTypeAndHeight.data.push_back(0.0);
        if (_curStatus.liftState != lift_state_down)
        {
            RCLCPP_INFO(node->get_logger(),"publishing fork down topic and lift_state is : %d", _curStatus.liftState);
            _pub_lift_speed_and_distance->publish(liftTypeAndHeight);
        }
        else
        {
            _screen_pub_mode = 0;
        }
    }
}

void CStateDiagram::Stop_Carto_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    
    int iRet = 0, hwStatus;
    RCLCPP_INFO(node->get_logger(),"begincmd=%s", msg->data.c_str());
    do
    {
        if (msg->data == "start_carto") //开始建图
        {
            if ((_statePub != ubt_Wait) && (_statePub != ubt_WaitPose) && (_statePub != ubt_Rmctl))
            {
                iRet = -1;
                break;
            }
            _curTask = CTaskMap::CreateTask(this, "", NULL);
            if (!_curTask->Start())
            {
                iRet = -4;
                _curTask = NULL;
                break;
            }
            _curTaskId = "create_map";
            _needArrived = false;
            _SetObsParams(1);
            _SetPoseState(pose_state_waitloc);
            _SetState(ubt_diagram_run);
            break;
        }
        if (msg->data == "stop_carto") //结束建图
        {
            if (_curTask == NULL)
            {
                iRet = -101;
                break;
            }
            if (_curTask->GetType() != ubt_task_type_map)
            {
                iRet = -102;
                break;
            }
            _SetPoseState(pose_state_waitloc);
            _curTask->Stop(ubt_task_result_usr_cancel);
            break;
        }
    }
    while (false);
    RCLCPP_INFO(node->get_logger(),"iRet=%d cmd=%s", iRet, msg->data.c_str());
    
}

// 启动 / 普通点或充电任务
void CStateDiagram::Fixedpath_Task_Callback(const std_msgs::msg::String::SharedPtr msg)
{

    RCLCPP_INFO(node->get_logger(),"==================== start fixedpath task callback ====================");
    int iRet = 0, iRes, hwStatus;
    FixePathTaskInfo taskInfo;
    std::string taskInfoMsg = msg->data, failMsg, desc;
    RCLCPP_INFO(node->get_logger(),"cmd: %s", msg->data.c_str());
    do
    {
        iRes = ParseFixedPathTaskInfo(taskInfoMsg, &taskInfo, failMsg);
        if (iRes != 0)
        {
            desc = "parse req fail, " + failMsg;
            iRet = -1;
            NotifyTaskEvent(UBT_TASK_EVENT_REQ_BADPARAM_STR);
            break;
        }

        // Set stop dis and rack info for starting
        _SetStopDis(taskInfo);
        _SetRackInfo(taskInfo);

        if ((_statePub != ubt_Wait) && (_statePub != ubt_LiftUp) && (_statePub != ubt_DragUp) && (_statePub != ubt_ShelfUp))
        {
            iRet = -2;
            desc = "in bad state";
            NotifyTaskEvent(UBT_TASK_EVENT_SYS_BADSTATUS_STR);
            break;
        }

        int navType = 0;
#ifdef UBT_ALGS_QRCODE_SUPPORT
        if (_cfg.loc_alg == ubt_locate_alg_qrcode)
        {
            navType = 1;
        }
#endif
	/* Shine */
        navType = 1;
	/* End */

        _curTask = CTaskEx::CreateTask(this, "", msg->data, navType);
        if (_curTask == NULL)
        {
            iRet = -3;
            desc = "create task fail ";
            NotifyTaskEvent(UBT_TASK_EVENT_SYS_BADSTATUS_STR);
            break;
        }
        _NotifyTaskStatus(taskInfo.taskId, UBT_TASK_STATUS_INPROCESS_STR, 0, "", "create task ok");
        _curTaskId = taskInfo.taskId;
        if (taskInfo.path.size() != 0)
        {
            _needArrived = true;
        }
        else
        {
            _needArrived = false;
        }
        _curTaskFlag = 0;
        _is_finish_current_task = false;
        if (!_curTask->Start())
        {
            iRet = -4;
            desc = "start task fail ";
            NotifyTaskEvent(UBT_TASK_EVENT_SYS_BADSTATUS_STR);
            break;
        }

        _SetState(ubt_diagram_run);
        break;
    }
    while (false);
    if (iRet != 0)
    {
        _NotifyTaskStatus(taskInfo.taskId, UBT_TASK_STATUS_ERROR_STR, 0, "", desc);
    }
    RCLCPP_INFO(node->get_logger(),"iRet=%d cmd: %s", iRet, msg->data.c_str());
    
}
//任务暂停,恢复，取消
void CStateDiagram::UI_Command_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    
    int iRet = 0, stopType = 0;
    ubt_task_type taskType;
    // RCLCPP_INFO(node->get_logger(),"[ubt_state]CStateDiagram::UI_Command_Callback  begin cmd=%s pose=(%lf,%lf,%lf)",
    //          msg->data.c_str(), _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta);
    do
    {
        if (msg->data == "visual_servo")  // U 二维码精定位
        {
            RCLCPP_INFO(node->get_logger(),"msg->data is visual_servo");
            _visualFinePositioning();
           break;
        }
        if (msg->data == "mission_action_auto_guide")//自动寻线 mission_action_auto_guide
        {
            std_msgs::msg::String msgInit;
            msgInit.data = "1";
            _pub_init_find_rfid->publish(msgInit);
            break;
        }
        // if cmd is pause or recover, and state is auto line, do pause or continue
        if (msg->data == "mission_manual_pause")
        {
            _autoLinePatrolPause(true);
        }
        else if (msg->data == "mission_manual_recover")
        {
            _autoLinePatrolPause(false);
        }

        if (_curTask == NULL)
        {
            iRet = -1;
            break;
        }
        if (msg->data == "mission_cancel")
        {
            _curTask->Stop(ubt_task_result_usr_cancel);
            break;
        }
        if (msg->data == "dispatch_cancel")
        {
            _curTask->Stop(ubt_task_result_usr_dispatchcancel);
            break;
        }
        if (msg->data == "navigation_cancel")
        {
             _curTask->Stop(ubt_task_result_navigation_cancel);
            break;
        }

        int oldPauseFlag, pauseFlag;
        oldPauseFlag = _curTaskFlag & (ubt_task_flag_manualpause | ubt_task_flag_autopause);
        if (msg->data == "mission_pause")
        {
            _curTaskFlag |= ubt_task_flag_autopause;
        }
        else if (msg->data == "mission_recover")
        {
            _curTaskFlag &= ~ubt_task_flag_autopause;
        }
        else if (msg->data == "mission_manual_pause")
        {
            _curTaskFlag |= ubt_task_flag_manualpause;
        }
        else if (msg->data == "mission_manual_recover")
        {
            _curTaskFlag &= ~ubt_task_flag_manualpause;
        }
        else
        {
            iRet = -3;
            break;
        }
        pauseFlag = _curTaskFlag & (ubt_task_flag_manualpause | ubt_task_flag_autopause);
        if (oldPauseFlag == pauseFlag)
        {
            iRet = 1;
            break;
        }
        if (oldPauseFlag == 0)
        {
            if (pauseFlag != 0)
            {
                _curTask->Pause();
            }
        }
        else
        {
            if (pauseFlag == 0)
            {
                _curTask->Continue();
            }
        }
        _RefreshState();
    }
    while (false);

    RCLCPP_INFO(node->get_logger(),"iRet=%d cmd=%s _curTaskFlag=%x", iRet, msg->data.c_str(), _curTaskFlag);
    
}
//任务更新-交通管制
void CStateDiagram::Task_Limit_Index_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(),"CStateDiagram::Task_Limit_Index_Callback start");
    int iRet = 0, iRes;
    NavTaskLimitIndex taskLimit;
    std::string failMsg;
    RCLCPP_INFO(node->get_logger(),"cmd: %s", msg->data.c_str());
    do
    {
        iRes = ParseTaskLimitIndex(msg->data, &taskLimit, failMsg);
        if (iRes != 0)
        {
            RCLCPP_WARN(node->get_logger(),"parse fail  cmd: %s failMsg=%s",
                     msg->data.c_str(), failMsg.c_str());
            break;
        }
        if (_curTaskId != taskLimit.taskId)
        {
            RCLCPP_WARN(node->get_logger(),"taskId unmatch  taskId: %s , curTaskId=%s",
                     taskLimit.taskId.c_str(), _curTaskId.c_str());
            break;
        }
        _curTask->OnMsg(ubt_msg_type_nav_limit, &taskLimit);
    }
    while (false);
    RCLCPP_INFO(node->get_logger(),"CStateDiagram::Task_Limit_Index_Callback end");
}


void CStateDiagram::OnTimer()
{
    if (_curTask != NULL)
    {
        _curTask->OnTimer();
    }
#ifdef UBT_USLAM_SUPPORT
    if (_curStatus.poseState == pose_state_syncmap)
    {
        if (_SyncMap(_curMap))
        {
            _curStatus.poseState = pose_state_waitloc;
        }
    }
#endif
    if (_curState == ubt_diagram_initial)
    {
        _OnTimerInInitial();
    }
    else if (_curState == ubt_diagram_initpose)
    {
        _OnTimerInInitPose();
    }
    else
    {
    }
#ifdef UBT_LIFT_SUPPORT
    _CheckExpInfo(0);//检查本体膨胀信息
#endif
    //_PoseCheckerTimeout();
    if (_curState == ubt_diagram_closed)
    {
        if ((_lastState != ubt_diagram_closed) &&
                (_curTask == NULL) &&
                ((ubt_com::GetNowInMsecs() - _curStateEnterTime) >= 2000))
        {
            rclcpp::shutdown();  
            return;
        }
    }
    if (_statePub == ubt_Obstacle)
    {
        if ((ubt_com::GetNowInMsecs() - _statePubEnterTime) >= (_cfg.obstacle_alarm_duration * 1000))
        {
            if (_obstacle_alarm_cnt == 0)
            {
                _obstacle_alarm_cnt++;
                _PublishAlarmForObstacle();
            }
        }
    }
    if (!_RefreshState())
    {
        _PublishState();
    }
    if(_curTask)
    {
        std_msgs::msg::UInt32 msg;
        ubt_task_state taskState = _curTask->GetState();
        msg.data = taskState;
        _pub_agv_state->publish(msg);

        ubt_task_stage taskStage = _curTask->GetStage();
        msg.data = taskStage;
        _pub_agv_stage->publish(msg);
    }
    auto_find_Avoid();
    _PublishScreenMode();
}

// 自巡线/空跑 避障
void CStateDiagram::auto_find_Avoid()
{
    ObstacleParams obsParams;
    obsParams = _cfg.obsParams;
    if(!_curStatus.autoFindState && !_curStatus.isStandaloneMode)
    {
        return;
    }

    obsParams.stopDis = _cfg.obstacle_auto_find_dis;//设置自巡线的避障参数
    UbtStateUtilSetObstacleParams(obsParams);

    ubt_interface::msg::NavReq navReq;
    if (_curStatus.laserAvoidStatus == 0 )
    {
        _auto_find_obs = false;
        navReq.cmd = 3; // 表示continue
        _pub_nav_req->publish(navReq);

    }else if(_curStatus.laserAvoidStatus == 1)
    {
        _auto_find_obs = true;
        navReq.cmd = 2; // 2: pause
        _pub_nav_req->publish(navReq);
    }
    else if(_curStatus.laserAvoidStatus == 2)
    {
        navReq.cmd = 5; // 5：slowDown
        _pub_nav_req->publish(navReq);
    }
}
//矢量速度
void CStateDiagram::Cmd_Vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
    if (_curState == ubt_diagram_run)
    {
        _pub_instance_twist->publish(*msg);
    }
}
//ui发过来的速度（wifi控制），需要平滑的
void CStateDiagram::UI_Twist_Callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
    if ((_curState == ubt_diagram_wait) ||
            (_curTask != NULL) && (_curTask->GetType() == ubt_task_type_map))
    {
        _pub_smooth_twist->publish(*msg);
    }
}


//获取mrpt的实时定位
void CStateDiagram::Location_CallBack(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
    if(_current_pose_type == "slam" || _current_pose_type == "fusion")
    {
        _curStatus.curPose.x = msg->pose.pose.position.x;
        _curStatus.curPose.y = msg->pose.pose.position.y;
        tf2::Quaternion quat(0, 0, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
        _curStatus.curPose.theta = ubt_com::getQuaternionYaw(quat);
        _curStatus.curPoseTime = ubt_com::GetNowInMsecs();

        _pub_current_pose->publish(_curStatus.curPose);
        if (_curTask != NULL)
        {
            _curTask->OnMsg(ubt_msg_type_current_pose, &_curStatus.curPose);
        }
    }
}

#ifdef UBT_ALGS_QRCODE_SUPPORT
//获取二维码的实时定位
void CStateDiagram::Location_QrCode_CallBack(const ubt_interface::msg::Pose2dStamped::SharedPtr msg)
{
    if(_current_pose_type == "qrcode")
    {
        if ((_cfg.loc_alg != ubt_locate_alg_qrcode) ||
                (_curStatus.poseState < pose_state_waitloc))
        {
            return;
        }
        _curStatus.curPose.x = msg->pose2d.x;
        _curStatus.curPose.y = msg->pose2d.y;
        _curStatus.curPose.theta = msg->pose2d.theta;
        _curStatus.curPoseTime = ubt_com::GetNowInMsecs();
        _pub_current_pose->publish(_curStatus.curPose);
        if (msg->header.frame_id == "lost")
        {
            if (_curStatus.poseState != pose_state_waitloc)
            {
                _curStatus.curPoseCfd = 0.0;
                _SetPoseState(pose_state_waitloc);
                std_msgs::msg::String msgPoseState;
                msgPoseState.data = "NG";
                _pub_inital_state->publish(msgPoseState);
                if (_curTask != NULL)
                {
                    _curTask->OnMsg(ubt_msg_type_map_confidence, &_curStatus.curPoseCfd);
                }
                _RefreshState();
            }
        }
        else
        {
            if (_curStatus.poseState != pose_state_ok)
            {
                _curStatus.curPoseCfd = 1.0;
                _SetPoseState(pose_state_ok);
                std_msgs::msg::String msgPoseState;
                msgPoseState.data = "OK";
                _pub_inital_state->publish(msgPoseState);
                _RefreshState();
            }
            else
            {
                if (_curTask != NULL)
                {
                    _curTask->OnMsg(ubt_msg_type_current_pose, &_curStatus.curPose);
                }
            }
        }
    }
}

//获取货架二维码
void CStateDiagram::Rack_QrCode_CallBack(const ubt_interface::msg::Pose2dStamped::SharedPtr msg)
{
    _curStatus.rackTag = atof(msg->header.frame_id.c_str());
    _curStatus.rackPose.x = msg->pose2d.x * 0.001;
    _curStatus.rackPose.y = msg->pose2d.y * 0.001;
    _curStatus.rackPose.theta = msg->pose2d.theta / 180 * M_PI;
    _curStatus.rackPoseTime = ubt_com::GetNowInMsecs();
}

#endif

#if (PLAT_USE == PLAT_U1000)
//电机板状态字--急停
void CStateDiagram::Event_Emergency_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    if(_event_emergency_msg == msg->data)//保证只生效一次
    {
        return;
    }
    RCLCPP_INFO(node->get_logger(),"CStateDiagram::Event_Emergency_Callback start");
    _event_emergency_msg = msg->data;
    
    int oldMbStatus = _curStatus.mbStatus ;
    if (msg->data != 0) //急停按钮按下
    {
        this->_autoFindPauseFlag = false;

        // 停止单机（空跑）模式
        std_msgs::msg::Bool msgStandaloneMode;
        msgStandaloneMode.data = false;
        _pub_standalone_mode->publish(msgStandaloneMode);
        //停止自巡线
        std_msgs::msg::String msgInit;
        msgInit.data = "0";
        _pub_init_find_rfid->publish(msgInit);
        //停止特征扫描
        std_msgs::msg::UInt8 msgCharacteristic;
        msgCharacteristic.data = 0;
        _pub_characteristic->publish(msgCharacteristic);
        _is_characteristic = 0;

        _is_finish_current_task = true;

        _curStatus.mbStatus |= mb_status_flag_stop_alarm;
        if (!(oldMbStatus & mb_status_flag_stop_alarm))
        {
            RCLCPP_INFO(node->get_logger(),"CStateDiagram::Event_Emergency_Callback stop clicked pose=(%lf,%lf,%lf)",
            _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta);
            if (_curTask != NULL)
            {
                if (_curTask->GetType() != ubt_task_type_map)
                {
                    _curTask->Stop(ubt_task_result_usr_stopbtn);
                }
                _bStopTrigged = true;
            }
            _NotifyEvent(1, "");
        }
    }
    else   //急停按钮松开
    {
        _isRefault = false;//急停按下，无效循环可恢复故障
        refaultDetailMsg.data = ""; //
        _pub_refault_detail->publish(refaultDetailMsg);

        _curStatus.mbStatus &= ~mb_status_flag_stop_alarm;
        if (oldMbStatus & mb_status_flag_stop_alarm)
        {
            RCLCPP_WARN(node->get_logger(),"stop released pose=(%lf,%lf,%lf)",
                    _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta);
            _curStatus.chargingConnect = charging_connect_null;
            if (_curTask != NULL)
            {
                _curTask->OnMsg(ubt_msg_type_mb_status, &_curStatus.mbStatus);
            }
            else
            {
                _SetState(ubt_diagram_initial);
                return;
            }
        }

    }
    if (oldMbStatus != _curStatus.mbStatus)
    {
        _RefreshState();
    }

    

    _event_emergency_msg = msg->data;
    RCLCPP_INFO(node->get_logger(),"CStateDiagram::Event_Emergency_Callback end");
}
//复位-跟急停类似
void CStateDiagram::Event_Reset_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(),"CStateDiagram::Event_Reset_Callback start");

    this->_autoFindPauseFlag = false;

    // 停止单机（空跑）模式
    std_msgs::msg::Bool msgStandaloneMode;
    msgStandaloneMode.data = false;
    _pub_standalone_mode->publish(msgStandaloneMode);
    //停止自巡线
    std_msgs::msg::String msgInit;
    msgInit.data = "0";
    _pub_init_find_rfid->publish(msgInit);
    //停止特征扫描
    std_msgs::msg::UInt8 msgCharacteristic;
    msgCharacteristic.data = 0;
    _pub_characteristic->publish(msgCharacteristic);
    _is_characteristic = 0;

    _is_finish_current_task = true;


    int oldMbStatus = _curStatus.mbStatus ;
    _isRefault = false;//急停按下，无效循环可恢复故障
    refaultDetailMsg.data = ""; //
    _pub_refault_detail->publish(refaultDetailMsg);
    if (_curTask != NULL)
    {
        if (_curTask->GetType() != ubt_task_type_map)
        {
            _curTask->Stop(ubt_task_result_usr_stopbtn);
        }
        _bStopTrigged = true;
    }
    _NotifyEvent(1, "");
    _curStatus.mbStatus &= ~mb_status_flag_stop_alarm;
    if (oldMbStatus & mb_status_flag_stop_alarm)
    {
        RCLCPP_WARN(node->get_logger(),"stop released pose=(%lf,%lf,%lf)",
                    _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta);
        _curStatus.chargingConnect = charging_connect_null;
        if (_curTask != NULL)
        {
            _curTask->OnMsg(ubt_msg_type_mb_status, &_curStatus.mbStatus);
        }
        else
        {
            _SetState(ubt_diagram_initial);
            return;
        }
    }
    _RefreshState();
    RCLCPP_INFO(node->get_logger(),"CStateDiagram::Event_Reset_Callback end");
}

void CStateDiagram::Update_Callback(const std_msgs::msg::UInt8::SharedPtr msg)
{
    if(msg->data == 1)
    {
        _is_update = true;
    }
    else
    {
        _is_update = false;
    }
}
//蓝牙连接状态
void CStateDiagram::Ble_Conn_State_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    int oldMbStatus = _curStatus.mbStatus ;
    static  int mbStatus_count;//mbStatus变化次数;
    RCLCPP_INFO(node->get_logger(),"CStateDiagram::Ble_Conn_State_Callback start");
    RCLCPP_INFO_STREAM(node->get_logger(),"msg: " << msg->data);

    if(msg->data != 0)
    {
       _curStatus.mbStatus |= mb_status_flag_bluetooth_connected;//把第一个比特位置1
       mbStatus_count = 0;
    }
    else
    {
        if(mbStatus_count < 3)
        {
            mbStatus_count++;
        }
        else
        {
            _curStatus.mbStatus &= ~mb_status_flag_bluetooth_connected;//把第一个比特位清零
        }
    }
    _RefreshState();
    if (_agv_type.substr(0, 5) == "f1200" || _agv_type.substr(0, 5) == "t3000") 
    {
        if (msg->data == 1 && _last_ble_con_state.data != 1)
        {
            _callAebEnable(false);
        }
    }
    _last_ble_con_state.data = msg->data;
    RCLCPP_INFO(node->get_logger(),"CStateDiagram::Ble_Conn_State_Callback end, _curStatus.mbStatus: %d", _curStatus.mbStatus);
}
//电源板状态字
void CStateDiagram::Power_State_Int_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    bool bFault = false;
    
    RCLCPP_DEBUG(node->get_logger(),"msg->data= %d ", (int) msg->data);
    _curStatus.powerState = (int)msg->data;
    if ((_curStatus.powerState == power_state_off_going) ||
            (_curStatus.powerState == power_state_off)) //关机
    {
        RCLCPP_WARN(node->get_logger(),"power off");
        _SetState(ubt_diagram_closed);
        return ;
    }
    else if (_curStatus.powerState == power_state_on)
    {
        if (_curState == ubt_diagram_closed) //关机状态，切换到初始化状态
        {
            _SetState(ubt_diagram_initial);
        }
    }
    else
    {
    }
    RCLCPP_INFO(node->get_logger(),"CStateDiagram::Power_State_Int_Callback end");

}
//充电状态
void CStateDiagram::Charging_State_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    static int oldChargingState = -1;
    if ((int)msg->data == oldChargingState)
    {
        return;
    }
    _curStatus.chargingState = (int)msg->data;
    oldChargingState = _curStatus.chargingState;
    RCLCPP_INFO(node->get_logger(),"chargingState= %d old=%d", _curStatus.chargingState, oldChargingState);
}
//充电桩状态
void CStateDiagram::Charging_Pile_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    int oldChargingPile = _curStatus.chargingPile;
    _curStatus.chargingPile = (int)msg->data;
    RCLCPP_DEBUG(node->get_logger(),"msg->data= %d", (int)msg->data);
    if (_curStatus.chargingPile == oldChargingPile)
    {
        return;
    }
}
//充电连接
void CStateDiagram::Charging_Connect_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    int oldChargingConnect = _newChargingConnect;
    _newChargingConnect = (int)msg->data;
    if (_newChargingConnect == oldChargingConnect)
    {
        _charge_connect_count++;
    }else
    {
        _charge_connect_count = 0;
    }
    RCLCPP_DEBUG(node->get_logger(),"msg->data= %d", (int)msg->data);
    if(_charge_connect_count >=6)//超过6帧才更新充电连接状态
    {
        _charge_connect_count = 0;
        if(_curStatus.chargingConnect == charging_connect_wire && _newChargingConnect != charging_connect_wire )  //手动充电断开后不会立即改为空闲
        {
            return;
        }
        else
        {
            _curStatus.chargingConnect = _newChargingConnect;
        }
        if ((_curStatus.chargingConnect == charging_connect_wire) && (_curTask != NULL))  //手动充电，停止所有任务
        {
            RCLCPP_INFO(node->get_logger(),"stop task stage=%d", _curTask->GetStage());
            _curTask->Stop(ubt_task_result_sys_charge);

        }
        else if ((_curStatus.chargingConnect == charging_connect_bed) &&
                (_curTask != NULL) &&
                (_curTask->GetStage() != ubt_task_stage_recharge2) &&
                (_curTask->GetStage() != ubt_task_stage_charge))
        {
            RCLCPP_INFO(node->get_logger(),"stop task stage=%d", _curTask->GetStage());
            _curTask->Stop(ubt_task_result_sys_charge);
        }
        _RefreshState();
    }
}

#else
void CStateDiagram::Mb_Status_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    int oldMbStatus = _curStatus.mbStatus ;
    _curStatus.mbStatus = (int)msg->data;
    RCLCPP_INFO_STREAM(node->get_logger(),"[ubt_state]CStateDiagram::Mb_Status_Callback msg: " << msg->data);
    if (msg->data & mb_status_flag_nrfault)
    {
        ROS_WARN_STREAM("[ubt_state]CStateDiagram::Mb_Status_Callback  recv nrfault msg: " << msg->data);
        if (_curState != ubt_diagram_nrfault)
        {
            _SetState(ubt_diagram_nrfault);
        }
        return;
    }
    if (msg->data & mb_status_flag_stop_alarm) //急停按钮按下
    {
        if (!(oldMbStatus & mb_status_flag_stop_alarm))
        {
            ROS_WARN_STREAM("[ubt_state]CStateDiagram::Mb_Status_Callback  stop clicked");
            if (_curTask != NULL)
            {
                if (_curTask->GetType() != ubt_task_type_map)
                {
                    _curTask->Stop(ubt_task_result_usr_stopbtn);
                }
                _bStopTrigged = true;
            }
            _NotifyEvent(1, "");
        }
    }
    else   //急停按钮松开
    {
        if (oldMbStatus & mb_status_flag_stop_alarm)
        {
            ROS_WARN_STREAM("[ubt_state]CStateDiagram::Mb_Status_Callback  stop released");
            if (_curTask != NULL)
            {
                _curTask->OnMsg(ubt_msg_type_mb_status, &_curStatus.mbStatus);
            }
            else
            {
                _SetState(ubt_diagram_initial);
                return;
            }
        }
    }
    if (oldMbStatus != _curStatus.mbStatus)
    {
        _RefreshState();
    }
}

void CStateDiagram::Pb_Status_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    bool bFault = false;
    RCLCPP_DEBUG(node->get_logger(),"[ubt_state]CStateDiagram::Pb_Status_Callback msg->data= %d ", (int) msg->data);
    _curStatus.pbStatus = (int)msg->data;
    if (msg->data & pb_status_flag_poweroff) //关机
    {
        RCLCPP_WARN(node->get_logger(),"[ubt_state]CStateDiagram::Pb_Status_Callback power off");
        _SetState(ubt_diagram_closed);
        return ;
    }
    if (msg->data & pb_status_flag_batteryexception) //电池通讯异常
    {
        RCLCPP_WARN(node->get_logger(),"[ubt_state]CStateDiagram::Pb_Status_Callback battery communicate  exception");
        _SetState(ubt_diagram_nrfault);
        return;
    }
    if (_curState == ubt_diagram_closed) //关机状态，切换到初始化状态
    {
        _SetState(ubt_diagram_initial);
    }
}

void CStateDiagram::Pb_Charge_Status_Callback(const std_msgs::msg::UInt8::SharedPtr msg)
{
    int oldPbChargeStatus = _curStatus.pbChargeStatus;
    _curStatus.pbChargeStatus = (int)msg->data;
    RCLCPP_DEBUG(node->get_logger(),"[ubt_state]CStateDiagram::Pb_Charge_Status_Callback msg->data= %d", (int)msg->data);
    if (_curStatus.pbChargeStatus == oldPbChargeStatus)
    {
        return;
    }
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CStateDiagram::Pb_Charge_Status_Callback pbChargeStatus= %d old=%d", _curStatus.pbChargeStatus, oldPbChargeStatus);
    if ((oldPbChargeStatus == 0) &&
            (_curTask != NULL) &&
            (_curTask->GetStage() != ubt_task_stage_recharge2) &&
            (_curTask->GetStage() != ubt_task_stage_charge))
    {
        RCLCPP_INFO(node->get_logger(),"[ubt_state]CStateDiagram::Pb_Charge_Status_Callback stop task stage=%d", _curTask->GetStage());
        _curTask->Stop(ubt_task_result_sys_charge);
    }
    else
    {
        _RefreshState();
    }
}
#endif
//防撞条事件
void CStateDiagram::Event_Collision_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    bool  newAntcTrigged = msg->data == 0 ? false : true;
    if (newAntcTrigged == _curStatus.antcTrigged)
    {
        return;
    }
    
    RCLCPP_INFO(node->get_logger(),"msg= %d pose=(%lf,%lf,%lf)",
             msg->data, _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta);
    _curStatus.antcTrigged = newAntcTrigged;

    this->_autoFindPauseFlag = false;

    // 停止单机（空跑）模式
    std_msgs::msg::Bool msgStandaloneMode;
    msgStandaloneMode.data = false;
    _pub_standalone_mode->publish(msgStandaloneMode);
    //取消自巡线
    std_msgs::msg::String msgInit;
    msgInit.data = "0";
    _pub_init_find_rfid->publish(msgInit);
    //
    if (newAntcTrigged)
    {
        if (_curTask != NULL)
        {
            if (_curTask->GetType() != ubt_task_type_map)
            {

                _curTask->Stop(ubt_task_result_sys_antc);
            }
        }
        _NotifyEvent(1, "");
    }
    _RefreshState();
    
}
//抱闸状态
void CStateDiagram::Event_Brake_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{
    bool  breakOpened = msg->data == 0 ? true : false;
    if (breakOpened == _curStatus.breakOpened)
    {
        return;
    }
    
    RCLCPP_INFO(node->get_logger(),"msg= %d  pose=(%lf,%lf,%lf)",
             msg->data, _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta);
    _curStatus.breakOpened = breakOpened;
    if (breakOpened)
    {
        if (_curTask != NULL)
        {
            if (_curTask->GetType() != ubt_task_type_map)
            {
                _curTask->Stop(ubt_task_result_usr_brakebtn);
            }
        }
        _NotifyEvent(1, "");
    }
    _RefreshState();
    
}
//整机升起状态-先用解抱闸状态
void CStateDiagram::Event_lifting_Callback(const std_msgs::msg::UInt32::SharedPtr msg)
{

    bool  breakOpened = msg->data != 1 ? true : false;
    if (breakOpened == _curStatus.breakOpened)
    {
        return;
    }
    
    RCLCPP_INFO(node->get_logger(),"msg= %d  pose=(%lf,%lf,%lf)",
             msg->data, _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta);
    _curStatus.breakOpened = breakOpened;
    if (breakOpened)
    {
        if (_curTask != NULL)
        {
            if (_curTask->GetType() != ubt_task_type_map)
            {
                _curTask->Stop(ubt_task_result_usr_brakebtn);
            }
        }
        _NotifyEvent(1, "");
    }
    _RefreshState();
    
}

#ifdef UBT_LIFT_SUPPORT
//顶升状态
void CStateDiagram::Event_Limit_CallBack(const std_msgs::msg::UInt32::SharedPtr msg)
{
    int newState = (int)msg->data;
    if (newState == _curStatus.liftState)
    {
        return;
    }
    
    RCLCPP_INFO_STREAM(node->get_logger(),"msg: " << msg->data);
    _curStatus.liftState = newState;
    if (_curStatus.liftState == lift_state_mid)
    {
        return;
    }
    std_msgs::msg::Float32 msg3;
    if (_curStatus.liftState == lift_state_up)
    {
        msg3.data = 0.9;
    }
    else
    {
        msg3.data = 0.15;
    }
    _pub_rgbd_high_height->publish(msg3);
    _CheckExpInfo(1);
    
}
#endif
#ifdef UBT_DRAG_SUPPORT
void CStateDiagram::Event_Drag_CallBack(const std_msgs::msg::UInt32::SharedPtr msg)
{
    int newState = (int)msg->data;
    if (newState == _curStatus.dragState)
    {
        return;
    }
    if(newState == drag_state_error)
    {
        refaultDetailMsg.data = "drag error";
        _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ERROR_STR, 0, "", refaultDetailMsg.data);
        std_msgs::msg::String newMsg;
        newMsg.data = "navigation_cancel";
        _pub_ui_state->publish(newMsg);
        _isRefault = true;//变成可恢复故障-先和充电失败用一样的
        return;
    }
    
    RCLCPP_INFO_STREAM(node->get_logger(),"msg: " << msg->data);
    _curStatus.dragState = newState;
    _CheckExpInfo(0);
    _RefreshState();
    
}
void CStateDiagram::traction_latch_abnormal_state_CallBack(const std_msgs::msg::UInt32::SharedPtr msg)
{
    if(msg->data >= 1)
    {
        refaultDetailMsg.data = "drag error";
        _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ERROR_STR, 0, "", refaultDetailMsg.data);
        std_msgs::msg::String newMsg;
        newMsg.data = "navigation_cancel";
        _pub_ui_state->publish(newMsg);
        _isRefault = true;//变成可恢复故障-先和充电失败用一样的

    }
}

#endif

void CStateDiagram::Wifi_Info_Callback(const ubt_interface::msg::WifiInfo::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(),"msg:  %d",msg->netstatus);
    _curStatus.netStatus = msg->netstatus;
}
//电池电量获取
void CStateDiagram::Batterydata_Callback(const ubt_interface::msg::BatteryData::SharedPtr msg)
{
    _curStatus.curPerPower = msg->perpower;
    if (_statePub == ubt_ChargeManual)
    {
        _RefreshState();
    }
}
//liq的避障模块
void CStateDiagram::Obstacle_Laser_Callback(const std_msgs::msg::UInt8::SharedPtr msg)
{
    RCLCPP_INFO_STREAM(node->get_logger(),"msg:	" << msg);
    _curStatus.laserAvoidStatus = msg->data;
    _avoid_laser_state_stime = ubt_com::GetNowInMsecs();
    _CheckObstacleStatus();
}

void CStateDiagram::Ui_characteristic_Callback(const std_msgs::msg::UInt8::SharedPtr msg)
{
    _is_characteristic = msg->data;
    _pub_characteristic->publish(*msg);
}
void CStateDiagram::Rfid_Callback(const ubt_interface::msg::Rfid::SharedPtr msg)
{
    //如果等待定位状态时，就在rfid点上就定位成功
    if(msg->is_induction == 1 && _statePub == ubt_WaitPose && _current_pose_type == "mag")
    {
        std_msgs::msg::String::SharedPtr rfid_msg;
        rfid_msg->data =  msg->id;
        _pub_InitialPose = true;
        Init_Find_Res_Callback(rfid_msg);
        _pub_InitialPose = true;
    }

    if(msg->is_induction == 1 && _is_characteristic == 1 && _characteristic_rfid != msg->id)  // 特征扫描
    {
        ubt_interface::msg::Characteristic pubMsg;
        pubMsg.type = "RFID";
        pubMsg.tag = msg->id;
        pubMsg.slam_x = _curStatus.curPose.x;
        pubMsg.slam_y = _curStatus.curPose.y;
        _pub_characteristic_res->publish(pubMsg);
        _characteristic_rfid = msg->id;//保证此rfid只上报一次
    }

    // mag 模式，直接在这里发布current_pose
    if (_current_pose_type == "mag" && msg->is_induction == 1)
    {
        for (auto map_popint : _map_points)
        {
            if (map_popint.rfid == msg->id)
            {
                geometry_msgs::msg::Pose2D curPose;
                curPose.x = map_popint.x;
                curPose.y = map_popint.y;
                curPose.theta = map_popint.t;
                // pub current pose
                _pub_current_pose->publish(curPose);
                break;
            }
        }
    }
}

void CStateDiagram::Obstacle_Rgbd_Callback(const std_msgs::msg::UInt8::SharedPtr msg)
{
    RCLCPP_INFO_STREAM(node->get_logger(),"msg: " << msg);
    _curStatus.rgbdAvoidStatus = msg->data;
    _avoid_rgbd_state_stime = ubt_com::GetNowInMsecs();
    _CheckObstacleStatus();
}

void CStateDiagram::Joystick_Status_Callback(const std_msgs::msg::Bool::SharedPtr msg)
{
    RCLCPP_INFO_STREAM(node->get_logger(),"msg: " << msg);
    if (msg->data)
    {
        _curStatus.joystickStatus = 1;
    }
    else
    {
        _curStatus.joystickStatus = 0;
    }
    _RefreshState();
}

// T1000单机模式
void CStateDiagram::Standalone_Mode_Callback(const std_msgs::msg::Bool::SharedPtr msg)
{
    if (this->_autoFindPauseFlag) // if is pause state then return
    {
        return;
    }
    _curStatus.isStandaloneMode = msg->data ? true : false;
}

//自动线状态
void CStateDiagram::Init_Find_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    if (this->_autoFindPauseFlag) // if is pause state then return
    {
        return;
    }

    if(msg->data == "1")
    {
        _curStatus.autoFindState = true;
    }
    else if(msg->data == "0")
    {
       _curStatus.autoFindState = false;
    }
}

//自动寻找rfid初始化
void CStateDiagram::Init_Find_Res_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    
    if(msg->data == "0")//初始化失败
    {
        std_msgs::msg::String code_msg;
        code_msg.data = "20011";
        _pub_msg_warn->publish(code_msg);
    }
    else//返回的rfid
    {
        for(int i = 0;i<_map_points.size();i++)
        {
            if(_map_points[i].rfid == msg->data)
            {
                _curStatus.curPose.x = _map_points[i].x;
                _curStatus.curPose.y = _map_points[i].y;
                _curStatus.curPose.theta = _map_points[i].t;

                if(_pub_InitialPose)
                {
                    _PublishInitialPose(_curStatus.curPose);
                    if(_current_pose_type == "slam" || _current_pose_type == "fusion")//如果有salm就只发送初始位置的一次
                    {
                        _pub_InitialPose = false;
                    }
                }

                _pub_current_pose->publish(_curStatus.curPose);
                std_msgs::msg::String::SharedPtr pose_msg;
                pose_msg->data = "OK";
                _curStatus.poseState = pose_state_locating;
                _SetState(ubt_diagram_wait);
                UI_PoseConfirm_Callback(pose_msg);
                
                break;
            }
        }
    }
    _curStatus.autoFindState = false;
    
}


#ifdef UBT_USLAM_SUPPORT
void CStateDiagram::InitialPoseUslam_Callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
    if (_cfg.loc_alg != ubt_locate_alg_uslam)
    {
        RCLCPP_WARN(node->get_logger(),"[ubt_state]CStateDiagram::InitialPoseUslam_Callback fail,  loc alg is not uslam");
        return;
    }
    tf::Quaternion quat(0, 0, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
    double theta = tf::getYaw(quat);
    relocalization_module_pkg::EstimateRelocalization::Request req;
    relocalization_module_pkg::EstimateRelocalization::Response rsp;
    bool bRes;
    req.initial_pose = msg;
    req.initial_pose.header.frame_id = "map";
    req.large_scale_flag = true;
    long beginTm = ubt_com::GetNowInMsecs();
    bRes = _cli_initnode_belpose.call(req, rsp);
    long cost = ubt_com::GetNowInMsecs() - beginTm;
    if (!bRes)
    {
        RCLCPP_WARN(node->get_logger(),"[ubt_state]CStateDiagram::InitialPoseUslam_Callback fail pose=(%lf,%lf,%lf) cost=%ld",
                 msg->pose.pose.position.x, msg->pose.pose.position.y, theta, cost);
        return;
    }
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CStateDiagram::InitialPoseUslam_Callback ok pose=(%lf,%lf,%lf) bel=%f cost=%ld",
             msg->pose.pose.position.x, msg->pose.pose.position.y, theta, rsp.estimate_score, cost);
    geometry_msgs::msg::Pose2D poseOpt;
    poseOpt.x = rsp.optimized_pose.pose.pose.position.x;
    poseOpt.y = rsp.optimized_pose.pose.pose.position.y;
    tf::Quaternion quat2(0, 0, rsp.optimized_pose.pose.pose.orientation.z, rsp.optimized_pose.pose.pose.orientation.w);
    poseOpt.theta = tf::getYaw(quat2);
    _PublishInitialPose(poseOpt);
}

bool CStateDiagram::_SyncMap(nav_msgs::msg::OccupancyGrid& map)
{
    maps_manager::SetSlamMap::Request req;
    maps_manager::SetSlamMap::Response rsp;
    req.slamMap = map;
    long startTm = ubt_com::GetNowInMsecs();
    bool bRes = _cli_uslam_setmap.call(req, rsp);
    long cost = ubt_com::GetNowInMsecs() - startTm;
    if (!bRes)
    {
        RCLCPP_WARN(node->get_logger(),"[ubt_state]CStateDiagram::_SyncMap  set map fail , cost=%ld", cost);
    }
    else
    {
        RCLCPP_INFO(node->get_logger(),"[ubt_state]CStateDiagram::_SyncMap  set map ok , cost=%ld", cost);
    }
    return bRes;
}
#endif
//地图明细
void CStateDiagram::Map_Confidence_Callback(const std_msgs::msg::Float32::SharedPtr msg)
{
    if(_current_pose_type == "slam" || _current_pose_type == "fusion")
    {
        _curStatus.curPoseCfd = msg->data;
        if (_curStatus.poseState == pose_state_ok)
        {
            _curStatus.curPoseCfdTime = ubt_com::GetNowInMsecs();
            if (_curStatus.curPoseCfd <= _cfg.pose_fail_bel_threshold)
            {
                RCLCPP_INFO(node->get_logger(),"in ok ,msg= %f pose_fail_bel_threshold=%f",
                        msg->data, _cfg.pose_fail_bel_threshold);
                _SetPoseState(pose_state_waitloc);
                if (_curTask != NULL)
                {
                    _curTask->OnMsg(ubt_msg_type_map_confidence, &_curStatus.curPoseCfd);
                }
                _RefreshState();
            }
            //确定位置
            _curStatus.poseState = pose_state_ok;


        }
        if (_curStatus.poseState == pose_state_locating)
        {
            if ((ubt_com::GetNowInMsecs() - _curStatus.curPoseCfdTime) < 3000)
            {
                return;
            }
            RCLCPP_INFO(node->get_logger(),"in posing ,msg= %f  _curState=%d", msg->data, _curState);
            _curStatus.curPoseCfdTime = ubt_com::GetNowInMsecs();
            std_msgs::msg::String msgPoseState;
            if (_curState == ubt_diagram_initpose)
            {
                if (_curStatus.curPoseCfd < _cfg.first_locate_bel_threshold)
                {
                    msgPoseState.data = "NG";
                    _SetPoseState(pose_state_waitloc);
                }
                else
                {
                    msgPoseState.data = "OK";
                    _SetPoseState(pose_state_ok);
                }
                _pub_inital_state->publish(msgPoseState);
                _SetState(ubt_diagram_wait);
            }
        }
    }
}
//获取地图所有点
void CStateDiagram::Muti_Map_List_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    std::string failMsg;
    if(msg->data != "")
    {
         _ParsePointsFromJson(msg->data.c_str(),&_map_points,failMsg);
    }

}

void CStateDiagram::Nav_Fail_CallBack(const std_msgs::msg::UInt32::SharedPtr msg)
{   
    // 1 : 偏航 2：牽引銷下降不允許大角度轉動
    if (msg->data == 1)
    {
        refaultDetailMsg.data = "track out";
    }
    else
    {
        refaultDetailMsg.data = "excessive turning angle";
    }

    _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ERROR_STR, 0, "", refaultDetailMsg.data);
    std_msgs::msg::String newMsg;
    newMsg.data = "navigation_cancel";
    _pub_ui_state->publish(newMsg);

    _isRefault = true;
}


//更新地图
void CStateDiagram::UI_MapUpdate_Callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(),"set map ");
    if (_curTask != NULL)
    {
        _curTask->Stop(ubt_task_result_sys_mapupdate);
    }
    switch (_cfg.loc_alg)
    {
#ifdef UBT_USLAM_SUPPORT
        case  ubt_locate_alg_uslam:
        {
            _curMap = msg;
            _SetPoseState(pose_state_syncmap);
            break;
        }
#endif
        default:
        {
            _SetPoseState(pose_state_waitloc);
        }
    }
    _RefreshState();
}
void CStateDiagram::Track_out_CallBack(const std_msgs::msg::String::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(),"msg= %s poseState=%d ", msg->data.c_str(), _curStatus.poseState);
    
    refaultDetailMsg.data = "track out";
    _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ERROR_STR, 0, "", refaultDetailMsg.data);
    std_msgs::msg::String newMsg;
    newMsg.data = "navigation_cancel";
    _pub_ui_state->publish(newMsg);

    
    _isRefault = true;
}
void CStateDiagram::Mqtt_Status_CallBack(const std_msgs::msg::UInt8::SharedPtr msg)
{
    if(msg->data == 2)
    {
        RCLCPP_INFO(node->get_logger(),"msg= %d poseState=%d ", msg->data, _curStatus.poseState);
        
        refaultDetailMsg.data = "mqtt connect lost";
        _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ERROR_STR, 0, "", refaultDetailMsg.data);
        std_msgs::msg::String newMsg;
        newMsg.data = "navigation_cancel";
        _pub_ui_state->publish(newMsg);
        
        _isRefault = true;
    }
}
void CStateDiagram::Top_Laser_Com_CallBack(const std_msgs::msg::UInt32::SharedPtr msg)
{
    int flag = (int)msg->data;
    if(flag)
    {
        RCLCPP_INFO(node->get_logger(),"msg= %d poseState=%d ", msg->data, _curStatus.poseState);
        _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ERROR_STR, 0, "", "lidar disconnected or no cloudpoint data");
        std_msgs::msg::String newMsg;
        newMsg.data = "navigation_cancel";
        _pub_ui_state->publish(newMsg);

        _SetState(ubt_diagram_nrfault);//变成不可恢复故障 -先和充电失败用一样的
    }
}
void CStateDiagram::Left_Laser_Com_CallBack(const std_msgs::msg::UInt32::SharedPtr msg)
{
    int flag = (int)msg->data;
    if(flag)
    {
        RCLCPP_INFO(node->get_logger(),"msg= %d poseState=%d ", msg->data, _curStatus.poseState);
        _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ERROR_STR, 0, "", "lidar disconnected or no cloudpoint data");
        std_msgs::msg::String newMsg;
        newMsg.data = "dispatch_cancel";
        _pub_ui_state->publish(newMsg);

        _SetState(ubt_diagram_nrfault);//变成不可恢复故障 -先和充电失败用一样的
    }
}
void CStateDiagram::Right_Laser_Com_CallBack(const std_msgs::msg::UInt32::SharedPtr msg)
{
    int flag = (int)msg->data;
    if(flag)
    {
        RCLCPP_INFO(node->get_logger(),"msg= %d poseState=%d ", msg->data, _curStatus.poseState);
        _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ERROR_STR, 0, "", "lidar disconnected or no cloudpoint data");
        std_msgs::msg::String newMsg;
        newMsg.data = "dispatch_cancel";
        _pub_ui_state->publish(newMsg);

        _SetState(ubt_diagram_nrfault);//变成不可恢复故障 -先和充电失败用一样的
    }
}
void CStateDiagram::end_rfid_broken_CallBack(const std_msgs::msg::String::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(),"msg= %s poseState=%d ", msg->data.c_str(), _curStatus.poseState);

    refaultDetailMsg.data = "last rfid broken";
    _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ERROR_STR, 0, "", refaultDetailMsg.data);
    std_msgs::msg::String newMsg;
    newMsg.data = "navigation_cancel";
    _pub_ui_state->publish(newMsg);

    _isRefault = true;
}

void CStateDiagram::rfidNotAtPath(const std_msgs::msg::UInt8::SharedPtr msg)
{
    if (msg->data == 1)
    {
        refaultDetailMsg.data = "last rfid broken";
        _NotifyTaskStatus(_curTaskId, UBT_TASK_STATUS_ERROR_STR, 0, "", refaultDetailMsg.data);
        std_msgs::msg::String newMsg;
        newMsg.data = "dispatch_cancel";
        _pub_ui_state->publish(newMsg);

        _isRefault = true;
    }
}

//是否完成了当前任务
void CStateDiagram::Finish_task_CallBack(const std_msgs::msg::UInt8::SharedPtr msg)
{
    if(msg->data == 1)
    {
        _is_finish_current_task = true;
    }
}

void CStateDiagram::_callAebEnable(bool isEnable)
{
    // ros::service::waitForService("/aeb/enable");
    // ubt_interface::srv::SetEnable se_srv;
    auto request = std::make_shared<ubt_interface::srv::SetEnable::Request>();
    request->enable = isEnable;
    request->disenable_time = 15.0;
    while (!_set_aeb_enable_client->wait_for_service(1s)) { 
        RCLCPP_INFO(node->get_logger(),"/aeb/enable service not available, waiting again...");
    }

    auto result = _set_aeb_enable_client->async_send_request(request); 
    if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)  
    {

        RCLCPP_INFO(node->get_logger(),"请求正常处理,响应结果: is success: %d, msg: %s",result.get()->success, result.get()->message.c_str());
    }
    else
    {

        RCLCPP_ERROR(node->get_logger(),"请求处理失败....");
        return;
    }

    // bool flag = _set_aeb_enable_client.call(se_srv);
    // if (flag)
    // {
    //     RCLCPP_INFO(node->get_logger(),"请求正常处理,响应结果: is success: %d, msg: %s",se_srv.response.success, se_srv.response.message.c_str());
    // }
    // else
    // {
    //     RCLCPP_ERROR(node->get_logger(),"请求处理失败....");
    //     return;
    // }
}

//屏幕暂停和启动切换
void CStateDiagram::Start_pause_CallBack(const std_msgs::msg::UInt8::SharedPtr msg)
{
    
    RCLCPP_INFO(node->get_logger(),"msg= %d poseState=%d ", msg->data, _curStatus.poseState);
    std_msgs::msg::String code_msg;

    if(_statePub ==  ubt_Run)
    {
        code_msg.data = "mission_manual_pause";
        _pub_ui_state->publish(code_msg);
    }
    else if(_statePub ==  ubt_PauseManual)
    {
        code_msg.data = "mission_manual_recover";
        _pub_ui_state->publish(code_msg);
    }
    else if(_statePub ==  ubt_Action)
    {
        std_msgs::msg::String state_msg;
        state_msg.data = "mission_action_wait_cancel";
        _pub_ui_state->publish(state_msg);
    }
    
}
//位置确定
void CStateDiagram::UI_PoseConfirm_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    
    RCLCPP_INFO(node->get_logger(),"msg= %s poseState=%d ", msg->data.c_str(), _curStatus.poseState);
    _curStatus.poseState = pose_state_locating;
    _SetState(ubt_diagram_wait);
    if ((_curStatus.poseState == pose_state_locating) &&
            (_curState != ubt_diagram_initpose))
    {
        std_msgs::msg::String msgPoseState;
        msgPoseState.data = "OK";
        _pub_inital_state->publish(msgPoseState);
        _SetPoseState(pose_state_ok);
        _RefreshState();
    }
    
}
//配置更新
void CStateDiagram::UI_ConfigUpdate_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(),"[CStateDiagram] UI_ConfigUpdate_Callback : msg= %s poseState=%d ", msg->data.c_str(), _curStatus.poseState);
    if (_curTask != NULL && _statePub != ubt_Wait && _statePub != ubt_WaitPose && _statePub != ubt_Close)
    {
        RCLCPP_WARN(node->get_logger(),"[CStateDiagram] UI_ConfigUpdate_Callback : There are task running, config not update!");
        // return ;
    }
    else
    {
        _LoadDyamicCfg();
    }
    _RefreshState();
}

void CStateDiagram::UI_BleSpeed_Callback(const std_msgs::msg::Int8MultiArray::SharedPtr msg)
{
    if (msg->data.size() == 0)
    {
        return;
    }
    float bleSpeed = (float)msg->data.at(0) / 10;
    _curStatus.bleSpeed = bleSpeed;
    RCLCPP_INFO(node->get_logger(),"bleSpeed=%f", bleSpeed);
}

void CStateDiagram::OdomPose_CallBack(const geometry_msgs::msg::Pose2D::SharedPtr msg)
{
    _curStatus.odomPose.x = msg->x;
    _curStatus.odomPose.y = msg->y;
    _curStatus.odomPose.theta = msg->theta;
    _curStatus.odomPoseTime = ubt_com::GetNowInMsecs();
}

void CStateDiagram::_InitForROS()
{
    
    std::string initial_pose_topic = "/initialpose";
#ifdef UBT_USLAM_SUPPORT
    if (_cfg.loc_alg == ubt_locate_alg_uslam)
    {
        initial_pose_topic = "/initialpose_accurate";
    }
#endif
    //pub
    _pub_inital_state = node->create_publisher<std_msgs::msg::String>("/inital_state", 1);
    _pub_initial = node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(initial_pose_topic, 1); //发布重定位命令
    _pub_instance_twist = node->create_publisher<geometry_msgs::msg::Twist>("/instance_cmd_vel", 1);
    _pub_smooth_twist = node->create_publisher<geometry_msgs::msg::Twist>("/smooth_cmd_vel", 1);

    _pub_state = node->create_publisher<std_msgs::msg::String>("/state", 1); //发布状态
    _pub_status = node->create_publisher<std_msgs::msg::String>("/ubt_state_status", 1); //发布详细状态
    _pub_current_pose = node->create_publisher<geometry_msgs::msg::Pose2D>("/current_pose", 1); //发布当前位姿
    _pub_task_event = node->create_publisher<std_msgs::msg::String>("/process_state", 10); //发布执行状态 yyy 2020-10-31 v3.2  UBT_TASK_EVENT_xxx
    _pub_reset_dispatch = node->create_publisher<std_msgs::msg::String>("/reset_dispatch", 1); //发布通知后台清空任务指令 yyy 2020-11-2 v3.2.1
    _pub_task_status = node->create_publisher<std_msgs::msg::String>("/task_status", 10);//发布任务状态
    _pub_obstacle_alarm = node->create_publisher<std_msgs::msg::String>("/ubt_state_o_obstacle_alarm", 1);//发布障碍物警告
    _pub_rmctl_mode = node->create_publisher<std_msgs::msg::UInt8>("/manual_control_allow", 1);//发布手动控制本体
    _pub_obstacle_exp = node->create_publisher<std_msgs::msg::Float32MultiArray>("/obstacle_rack", 1);//发布货架障碍信息
    _pub_rgbd_high_height = node->create_publisher<std_msgs::msg::Float32>("/rgbd_detect_high_height_info", 1);//发布rgbd探头信息
    _pub_msg_warn = node->create_publisher<std_msgs::msg::String>("/agv/diag_warn", 5);//发布警告信息
    _pub_ui_state = node->create_publisher<std_msgs::msg::String>("/ui_state", 5);//发布任务取消
    _pub_init_find_rfid = node->create_publisher<std_msgs::msg::String>("/init_find_rfid", 1);//ui发送寻找rfid定位--用来急停
    _pub_standalone_mode = node->create_publisher<std_msgs::msg::Bool>("/standalone_mode", 1); // 单机模式
    _pub_characteristic = node->create_publisher<std_msgs::msg::UInt8>("/characteristic", 1);//发布给底层去扫描
    _pub_characteristic_res = node->create_publisher<ubt_interface::msg::Characteristic>("/characteristic_res", 1);//回复数据给调度

    //测试-pub
    _pub_agv_state= node->create_publisher<std_msgs::msg::UInt32>("/agv_state", 1);//状态
    _pub_agv_stage = node->create_publisher<std_msgs::msg::UInt32>("/agv_stage", 1);//阶段
    // 发送导航请求-用于自巡线避障
    _pub_nav_req = node->create_publisher<ubt_interface::msg::NavReq>("/nav_req", 10);

    _pub_clean_recover_task   = node->create_publisher<std_msgs::msg::UInt8>("/clean_recover_task", 1);//清除可恢复的任务
    _pub_refault_detail   = node->create_publisher<std_msgs::msg::String>("/refault_detail", 1);//可恢复故障详细

#ifdef UBT_LIFT_SUPPORT
    _pub_lift_speed = node->create_publisher<std_msgs::msg::Float32>("/lift_speed", 1);//发布顶升速度--循环发lift_speed才有用
    _pub_lift_speed_and_distance = node->create_publisher<std_msgs::msg::Float32MultiArray>("/lift_speed_and_distance", 10);
    _sub_lift_speed_and_distance_screen = node->create_subscription<std_msgs::msg::Float32MultiArray>("/lift_speed_and_distance_screen", 1, std::bind(&CStateDiagram::Lift_Speed_And_Distance_Screen_Callback, this, std::placeholders::_1));
#endif

    // sub 来自python_demo
    _sub_stop_carto = node->create_subscription<std_msgs::msg::String>("/ui_stop_carto", 1, std::bind(&CStateDiagram::Stop_Carto_Callback, this, std::placeholders::_1));
    _sub_fixedpath_task = node->create_subscription<std_msgs::msg::String>("/ui_fixedpath_task", 3, std::bind(&CStateDiagram::Fixedpath_Task_Callback, this, std::placeholders::_1));
    _sub_ui_state = node->create_subscription<std_msgs::msg::String>("/ui_state", 10, std::bind(&CStateDiagram::UI_Command_Callback, this, std::placeholders::_1));
    _sub_task_limit_index = node->create_subscription<std_msgs::msg::String>("/task_limit_index", 1, std::bind(&CStateDiagram::Task_Limit_Index_Callback, this, std::placeholders::_1));

    _sub_cmd_vel = node->create_subscription<geometry_msgs::msg::Twist>("/cmd_vel", 1, std::bind(&CStateDiagram::Cmd_Vel_callback, this, std::placeholders::_1));
    _sub_ui_twist = node->create_subscription<geometry_msgs::msg::Twist>("/ui_cmd_vel", 1, std::bind(&CStateDiagram::UI_Twist_Callback, this, std::placeholders::_1));
    _sub_loc_pose = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/mrpt_pose", 1, std::bind(&CStateDiagram::Location_CallBack, this, std::placeholders::_1)); //获取mrpt的实时定位
#ifdef UBT_ALGS_QRCODE_SUPPORT
    _sub_loc_qrcode_pose = node->create_subscription<ubt_interface::msg::Pose2dStamped>("/pose2d_stamped", 1, std::bind(&CStateDiagram::Location_QrCode_CallBack, this, std::placeholders::_1));//获取二维码的实时定位
    _sub_rack_qrcode_pose = node->create_subscription<ubt_interface::msg::Pose2dStamped>("/robot_pose2d_in_shelf", 1, std::bind(&CStateDiagram::Rack_QrCode_CallBack, this, std::placeholders::_1)); //获取货架二维码
#endif

#if (PLAT_USE == PLAT_U1000)
    _sub_event_emergency = node->create_subscription<std_msgs::msg::UInt32>("/event_emergency", 1, std::bind(&CStateDiagram::Event_Emergency_Callback, this, std::placeholders::_1));
    _sub_event_reset = node->create_subscription<std_msgs::msg::UInt32>("/event_reset", 1, std::bind(&CStateDiagram::Event_Reset_Callback, this, std::placeholders::_1));
    _sub_ble_con = node->create_subscription<std_msgs::msg::UInt32>("/ble_conn_state", 1, std::bind(&CStateDiagram::Ble_Conn_State_Callback, this, std::placeholders::_1));
    _sub_power_state_int = node->create_subscription<std_msgs::msg::UInt32>("/power_state_int", 1, std::bind(&CStateDiagram::Power_State_Int_Callback, this, std::placeholders::_1));
    _sub_charging_state = node->create_subscription<std_msgs::msg::UInt32>("/charging_state", 1, std::bind(&CStateDiagram::Charging_State_Callback, this, std::placeholders::_1));
    _sub_charging_pile = node->create_subscription<std_msgs::msg::UInt32>("/charging_pile", 10, std::bind(&CStateDiagram::Charging_Pile_Callback, this, std::placeholders::_1));
    _sub_charging_connect = node->create_subscription<std_msgs::msg::UInt32>(TOPIC_CHARGING_CONNECT, 10, std::bind(&CStateDiagram::Charging_Connect_Callback, this, std::placeholders::_1));
#else
    _sub_mb_status = node->create_subscription<std_msgs::msg::UInt32>("/mb_status", 1, std::bind(&CStateDiagram::Mb_Status_Callback, this, std::placeholders::_1));
    _sub_pb_status = node->create_subscription<std_msgs::msg::UInt32>("/pb_status", 1, std::bind(&CStateDiagram::Pb_Status_Callback, this, std::placeholders::_1));
    _sub_pb_charge_status = node->create_subscription<std_msgs::msg::UInt8>("/pb_charge_status", 1, std::bind(&CStateDiagram::Pb_Charge_Status_Callback, this, std::placeholders::_1));
#endif
    _sub_event_collision = node->create_subscription<std_msgs::msg::UInt32>("/event_collision", 1, std::bind(&CStateDiagram::Event_Collision_Callback, this, std::placeholders::_1));
    _sub_event_brake = node->create_subscription<std_msgs::msg::UInt32>("/event_brake", 1, std::bind(&CStateDiagram::Event_Brake_Callback, this, std::placeholders::_1));
    _sub_event_lifting = node->create_subscription<std_msgs::msg::UInt32>("/lifting_jack_state", 1, std::bind(&CStateDiagram::Event_lifting_Callback, this, std::placeholders::_1));
    _sub_ui_characteristic = node->create_subscription<std_msgs::msg::UInt8>("/ui_characteristic", 1, std::bind(&CStateDiagram::Ui_characteristic_Callback, this, std::placeholders::_1));
    _sub_rfid = node->create_subscription<ubt_interface::msg::Rfid>("/rfid_state", 1, std::bind(&CStateDiagram::Rfid_Callback, this, std::placeholders::_1));
#ifdef UBT_LIFT_SUPPORT
    _sub_event_limit = node->create_subscription<std_msgs::msg::UInt32>("/event_limit", 1, std::bind(&CStateDiagram::Event_Limit_CallBack, this, std::placeholders::_1));
#endif
#ifdef UBT_DRAG_SUPPORT
    _sub_event_drag = node->create_subscription<std_msgs::msg::UInt32>("/traction_latch_state", 1, std::bind(&CStateDiagram::Event_Drag_CallBack, this, std::placeholders::_1));
    _sub_traction_latch_abnormal_state = node->create_subscription<std_msgs::msg::UInt32>("/traction_latch_abnormal_state", 1, std::bind(&CStateDiagram::traction_latch_abnormal_state_CallBack, this, std::placeholders::_1));
    
#endif
    _sub_wifi_info = node->create_subscription<ubt_interface::msg::WifiInfo>("/wifi_info", 1, std::bind(&CStateDiagram::Wifi_Info_Callback, this, std::placeholders::_1));
    _sub_battery_data = node->create_subscription<ubt_interface::msg::BatteryData>("/BatteryData", 1, std::bind(&CStateDiagram::Batterydata_Callback, this, std::placeholders::_1));
    _sub_obstacle_laser = node->create_subscription<std_msgs::msg::UInt8>("/lidar_obstacle_status", 1, std::bind(&CStateDiagram::Obstacle_Laser_Callback, this, std::placeholders::_1));
    _sub_obstacle_rgbd = node->create_subscription<std_msgs::msg::UInt8>("/rgbd_obstacle_status", 1, std::bind(&CStateDiagram::Obstacle_Rgbd_Callback, this, std::placeholders::_1));
    _sub_joystick_status = node->create_subscription<std_msgs::msg::Bool>("/joystick_connect", 1, std::bind(&CStateDiagram::Joystick_Status_Callback, this, std::placeholders::_1));
    _sub_map = node->create_subscription<nav_msgs::msg::OccupancyGrid>("/map", 1, std::bind(&CStateDiagram::UI_MapUpdate_Callback, this, std::placeholders::_1));
    _sub_init_rfid = node->create_subscription<std_msgs::msg::String>("/init_find_rfid", 1, std::bind(&CStateDiagram::Init_Find_Callback, this, std::placeholders::_1));
    _sub_init_rfid = node->create_subscription<std_msgs::msg::String>("/init_find_rfid_res", 1, std::bind(&CStateDiagram::Init_Find_Res_Callback, this, std::placeholders::_1));
    _sub_pose_confidence = node->create_subscription<std_msgs::msg::Float32>("/map_confidence", 1, std::bind(&CStateDiagram::Map_Confidence_Callback, this, std::placeholders::_1));
    _sub_muti_map_list = node->create_subscription<std_msgs::msg::String>("/muti_map_list", 1, std::bind(&CStateDiagram::Muti_Map_List_Callback, this, std::placeholders::_1));
    _sub_pose_confirm = node->create_subscription<std_msgs::msg::String>("/pose_confirm", 1, std::bind(&CStateDiagram::UI_PoseConfirm_Callback, this, std::placeholders::_1));
    _sub_update = node->create_subscription<std_msgs::msg::UInt8>("/ubt_update", 1, std::bind(&CStateDiagram::Update_Callback, this, std::placeholders::_1));
    _sub_ui_config_update = node->create_subscription<std_msgs::msg::String>("/ui_config_update", 1, std::bind(&CStateDiagram::UI_ConfigUpdate_Callback, this, std::placeholders::_1));
    _sub_ble_speed = node->create_subscription<std_msgs::msg::Int8MultiArray>("/ble_speed", 1, std::bind(&CStateDiagram::UI_BleSpeed_Callback, this, std::placeholders::_1));
    _sub_odom_pose = node->create_subscription<geometry_msgs::msg::Pose2D>("/real_odom", 1, std::bind(&CStateDiagram::OdomPose_CallBack, this, std::placeholders::_1));
    _sub_track_out = node->create_subscription<std_msgs::msg::String>("/track_out", 1, std::bind(&CStateDiagram::Track_out_CallBack, this, std::placeholders::_1));
    _sub_mqtt_status = node->create_subscription<std_msgs::msg::UInt8>("/mqtt_status", 1, std::bind(&CStateDiagram::Mqtt_Status_CallBack, this, std::placeholders::_1));
    _sub_top_laser_error = node->create_subscription<std_msgs::msg::UInt32>("/top_laser_com", 1, std::bind(&CStateDiagram::Top_Laser_Com_CallBack, this, std::placeholders::_1));
    _sub_nav_fail = node->create_subscription<std_msgs::msg::UInt32>("/nav_fail", 1, std::bind(&CStateDiagram::Nav_Fail_CallBack, this, std::placeholders::_1));
    _sub_left_laser_error = node->create_subscription<std_msgs::msg::UInt32>("/left_laser_com", 1, std::bind(&CStateDiagram::Left_Laser_Com_CallBack, this, std::placeholders::_1));
    _sub_right_laser_error = node->create_subscription<std_msgs::msg::UInt32>("/right_laser_com", 1, std::bind(&CStateDiagram::Right_Laser_Com_CallBack, this, std::placeholders::_1));
    _sub_end_rfid_broken = node->create_subscription<std_msgs::msg::String>("/end_rfid_broken", 1, std::bind(&CStateDiagram::end_rfid_broken_CallBack, this, std::placeholders::_1));
    _sub_rfid_not_at_path = node->create_subscription<std_msgs::msg::UInt8>("/rfid_not_at_path", 1, std::bind(&CStateDiagram::rfidNotAtPath,this, std::placeholders::_1));

    // _sub_ble_con = node->create_subscription<std_msgs::msg::UInt32>("/ble_conn_state", 1, std::bind(&CStateDiagram::Ble_con_CallBack, this, std::placeholders::_1));
    _sub_start_pause = node->create_subscription<std_msgs::msg::UInt8>("/start_pause", 1, std::bind(&CStateDiagram::Start_pause_CallBack, this, std::placeholders::_1));
    _sub_finish_task = node->create_subscription<std_msgs::msg::UInt8>("/finish_task", 1, std::bind(&CStateDiagram::Finish_task_CallBack, this, std::placeholders::_1));
    _sub_standalone_mode = node->create_subscription<std_msgs::msg::Bool>("/standalone_mode", 1, std::bind(&CStateDiagram::Standalone_Mode_Callback, this, std::placeholders::_1));
    node->declare_parameter("agv_type", "");
     _agv_type = node->get_parameter("agv_type").as_string();
    node->declare_parameter("current_pose_type", "slam");
     _current_pose_type = node->get_parameter("current_pose_type").as_string();

    //service
    _cli_odom_reset = node->create_client<ubt_interface::srv::OdomReset>("/odom_reset");
    _set_footprint_client = node->create_client<ubt_interface::srv::SetFootprint>("/aeb/set_footprint");
    // _set_stop_distance_client = node->create_client<aeb::SetStopDistance>("/aeb/set_stop_distance");
    _set_aeb_enable_client = node->create_client<ubt_interface::srv::SetEnable>("/aeb/enable");

    //timer
    _timer  = node->create_wall_timer(std::chrono::milliseconds(500),std::bind(&CStateDiagram::OnTimer, this));

    switch (_cfg.loc_alg)
    {
#ifdef UBT_USLAM_SUPPORT
        case ubt_locate_alg_uslam:
            _cli_initnode_belpose = _node->create_client<relocalization_module_pkg::EstimateRelocalization::Request>("/getRelocateEstimate");
            _cli_uslam_setmap = _node->create_client<maps_manager::SetSlamMap::Request>("/setSlamMap");
            _sub_uslam_pose = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/initialpose_uslam", 1, std::bind(&CStateDiagram::InitialPoseUslam_Callback, this, std::placeholders::_1));
            break;
#endif
        default:
            _cli_initnode_belpose = node->create_client<ubt_interface::srv::BelPose>("/bel_pose");
    }
    
}


static std::string g_StatePubStrArr[] =
{
    UBT_STATE_CLOSE_STR,
    UBT_STATE_INITIAL_STR,
    UBT_STATE_INITPOSE_STR,
    UBT_STATE_WAIT_STR,
    UBT_STATE_MAPPING_STR,
    UBT_STATE_RUN_STR,
    UBT_STATE_ACTION_STR,
    UBT_STATE_RECHARGE_STR,
    UBT_STATE_CHARGE_STR,
    UBT_STATE_OBSTACLE_STR,
    UBT_STATE_PAUSE_STR,
    UBT_STATE_STOP_STR,
    UBT_STATE_REFAULT_STR,
    UBT_STATE_NREFAULT_STR,
    UBT_STATE_RMCTL_STR,
    UBT_STATE_CHARGEMANUL_STR,
    UBT_STATE_PAUSEMANUL_STR,
    UBT_STATE_WAITPOSE_STR,
    UBT_STATE_BREAKOPENED_STR,
    UBT_STATE_COLLIDE_STR,
    UBT_STATE_LIFTUP_STR,
    UBT_STATE_DRAGUP_STR,
    UBT_STATE_PAUSELIMIT_STR,
    UBT_STATE_AUTOFIND_STR,
    UBT_STATE_UPDATE_STR,
    UBT_STATE_SHELFUP_STR,
    UBT_STATE_STANDALONE_MODE_STR,
};

/* 当前状态优先级
ubt_Close = 0, //初始状态
ubt_Initial,//硬件自检 1
ubt_Initpose,//初始定位 2
ubt_NRefault, //不可恢复故障(关机，电机异常等) 13
ubt_Stop, //急停按下 11
ubt_Collide, //碰撞中 19
ubt_Refault, //可恢复故障 12
ubt_ChargeManual, //手动充电 15
ubt_BreakOpened, //解抱闸 18
ubt_Charge, //充电(充电和正常下桩) 8
ubt_Mapping, // 4
Ubt_StandaloneMode, //单机模式
ubt_AutoFind, //自巡线
ubt_WaitPose, //待定位 17
ubt_Action, // 6
ubt_PauseManual, //手动暂停 16
ubt_Pause, //暂停 10
ubt_Obstacle, //避障 9
ubt_PauseLimit, //到点暂停
ubt_Recharge, //回充(导航和上桩) 7
ubt_Run, //运行中 5
ubt_LiftUp, //顶升中 20
ubt_DragUp, //牵引销顶升中
ubt_Rmctl, //遥控模式 14
ubt_Wait, //空闲 3*/

bool CStateDiagram::_RefreshState()
{
    ubt_state_pub oldStatePub = _statePub;
    ubt_task_state taskState;
    ubt_task_stage taskStage;
    int hwStatus, rmctlModeNew;
    long now = ubt_com::GetNowInMsecs();
    do
    {
        if(_is_update)
        {
            _statePub = ubt_Update;
            break;
        }
        if (_curState == ubt_diagram_closed)
        {
            _statePub = ubt_Close;
            break;
        }
        if (_curState == ubt_diagram_initial)
        {
            _statePub = ubt_Initial;
            break;
        }
        if (_curState == ubt_diagram_initpose)
        {
            _statePub = ubt_Initpose;
            break;
        }
        if (_curState == ubt_diagram_nrfault)
        {
            _statePub = ubt_NRefault;
            break;
        }
    	hwStatus = _GetHwStatus();
        if (hwStatus >= 11) 	//不可能出现情形
        {
            _statePub = ubt_NRefault;
            break;
        }
        if (hwStatus == 9)//急停优先
        {
            _statePub = ubt_Stop;
            break;
        }
        if (hwStatus == 10)
        {
            _statePub = ubt_Collide;
            break;
        }
        if(_isRefault)
        {
            _statePub = ubt_Refault;
            break;
        }
        if (hwStatus == 3)
        {
            _statePub = ubt_ChargeManual;
            break;
        }
        if (hwStatus == 2)
        {
            _statePub = ubt_BreakOpened;
            break;
        }
        if(_curTask != NULL)
        {
            taskStage = _curTask->GetStage();
            if(taskStage == ubt_task_stage_mapping)
            {
                _statePub  = ubt_Mapping;
                break;
            }
            else if(taskStage == ubt_task_stage_recharge_fail)
            {
                refaultDetailMsg.data = "recharge failed";
                _isRefault = true;
            }
        }
        if (_curStatus.autoFindState || _curStatus.isStandaloneMode)
        {
            _statePub = _curStatus.isStandaloneMode ? Ubt_StandaloneMode : ubt_AutoFind;
            if (this->_autoFindPauseFlag)
            {
                _statePub = ubt_PauseManual;
            }
            else if(_auto_find_obs)
            {
               _statePub = ubt_Obstacle;
            }
            break;
        }
        if (_curStatus.poseState != pose_state_ok)
        {
            _statePub = ubt_WaitPose;
            break;
        }
        if(_curTask == NULL)//当前任务为空时时，状态等级重排
        {
            if (hwStatus == 1)
            {
                _statePub = ubt_LiftUp;
            }
            else if(hwStatus == 5)
            {
                _statePub = ubt_ShelfUp;
            }
            else if(hwStatus == 4)
            {
                _statePub = ubt_DragUp ;
            }
            else if (
                    (((_curStatus.mbStatus & mb_status_flag_bluetooth_connected) != 0) || ((_curStatus.joystickStatus & joystick_status_flag_connected) != 0)))
            {
                _statePub = ubt_Rmctl;
            }
            else
            {
                // if(_is_finish_current_task)
                // {
                    _statePub = ubt_Wait;
                // }
                // else
                // {
                //     _statePub = ubt_Run;
                // }
            }
            break;
        }
    	taskState = _curTask->GetState();
        if (taskState == ubt_task_state_pause)
        {
            if ((_curTaskFlag & ubt_task_flag_manualpause) != 0)
            {
                _statePub = ubt_PauseManual;
            }
            else if ((_curTaskFlag & ubt_task_flag_autopause) != 0)
            {
                _statePub = ubt_Pause;
            }
            else
            {
                _statePub = ubt_PauseLimit;
            }
        }
        else if (taskState == ubt_task_state_obstacle)
        {
            _statePub = ubt_Obstacle;
        }
        else
        {
            if(taskStage == ubt_task_stage_charge)
            {
                _statePub = ubt_Charge;
            }
            else if(taskStage == ubt_task_stage_action && taskState == ubt_task_state_running)
            {
                _statePub  = ubt_Action;
            }
            else if ((taskStage == ubt_task_stage_recharge2 || 
                taskStage == ubt_task_stage_recharge) &&
                taskState != ubt_task_state_idle
            )
            {
                _statePub = ubt_Recharge;
            }
            else if(taskStage == ubt_task_stage_run && taskState == ubt_task_state_running)
            {
                _statePub  = ubt_Run;
            }
            else
            {

                if (hwStatus == 1)
                {
                    _statePub = ubt_LiftUp;
                }
                else if(hwStatus == 5)
                {
                    _statePub = ubt_ShelfUp;
                }
                else if(hwStatus == 4)
                {
                    _statePub = ubt_DragUp ;
                }
                else if (
                        (((_curStatus.mbStatus & mb_status_flag_bluetooth_connected) != 0) || ((_curStatus.joystickStatus & joystick_status_flag_connected) != 0)))
                {
                    _statePub = ubt_Rmctl;
                }
                else
                {
                    // if(_curTask == NULL)
                    _statePub = ubt_Wait;
                }
            }
        }

        if (_curState == ubt_diagram_wait)
        {
            if (_bStopTrigged)
            {
                _bStopTrigged = false;
            }
        }
    }
    while (false);
    //发布遥控模式
    rmctlModeNew = 0;
    if ((_statePub == ubt_Rmctl) ||
            (_statePub == ubt_Mapping))
    {
        if ((_curStatus.mbStatus & mb_status_flag_bluetooth_connected) != 0)
        {
            rmctlModeNew = 1;
        }
        if ((_curStatus.joystickStatus & joystick_status_flag_connected) != 0)
        {
            rmctlModeNew = 2;
        }
    }
    if (rmctlModeNew != _rmctlMode)
    {
        _rmctlMode = rmctlModeNew;
        _PublishRmctlMode();
    }

    //发布状态
    bool bRet = false;
    if (oldStatePub != _statePub)
    {
        RCLCPP_INFO(node->get_logger(),"update state _statePub= %d (%s) changed ,oldStatePub=  %d (%s) pose=(%lf,%lf,%lf)",
                 _statePub, g_StatePubStrArr[_statePub].c_str(), oldStatePub, g_StatePubStrArr[oldStatePub].c_str(),
                 _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta);
        _statePubEnterTime = now;
        _PublishState();
        if(_statePub == ubt_Refault)//发出可恢复故障详细
        {
            _pub_refault_detail->publish(refaultDetailMsg);
        }
        bRet = true;
    }
    return bRet;

}
//获取硬件状态,0 无故障,1 顶升到顶部或途中，2 松开解抱闸, 3  线充,4 牵引销到顶部或途中,5,货叉抬升,6,货叉在途中，9 按下急停,10 防撞条触发,11 左轮/右轮/滚筒电机报警,12
int CStateDiagram::_GetHwStatus()
{
    int iRet = 0;
    do
    {
#if (PLAT_USE == PLAT_U1000)
#else
        if (_curStatus.pbStatus & pb_status_flag_poweroff)
        {
            iRet = 13;
            break;
        }
        if (_curStatus.pbStatus & pb_status_flag_batteryexception)
        {
            iRet = 12;
            break;
        }
#endif
        if (_curStatus.mbStatus & mb_status_flag_nrfault) //左轮/右轮/滚筒电机报警
        {
            iRet = 11;
            break;
        }
        if (_curStatus.mbStatus & mb_status_flag_stop_alarm)//急停优先
        {
            iRet = 9;
            break;
        }
        if (_curStatus.antcTrigged)
        {
            iRet = 10;
            break;
        }

        if (_curStatus.breakOpened)
        {
            iRet = 2;
            break;
        }

        if (_curStatus.liftState == lift_state_mid || _curStatus.liftState == lift_state_up)
        {
            if (_agv_type == "f1200")
            {
                iRet = 5;
                break;
            }
            iRet = 1;
            break;
        }
        if (_curStatus.dragState == drag_state_mid || _curStatus.dragState == drag_state_up)
        {
            iRet = 4;
            break;
        }
#if (PLAT_USE == PLAT_U1000)
        if (_curStatus.chargingConnect == charging_connect_wire )
        {
            iRet = 3;
            break;
        }
#else
        if (_curStatus.pbChargeStatus != pb_charge_status_idle)
        {
            iRet = 3;
            break;
        }
#endif

    }
    while (false);
    return iRet;
}

void CStateDiagram::_SetState(ubt_diagram_state  newState)
{
    if (_curState == newState)
    {
        return;
    }
    
    RCLCPP_INFO(node->get_logger(),"oldState=%d newState=%d", _curState, newState);
    _lastState = _curState;
    _curState = newState;
    _curStateEnterTime = ubt_com::GetNowInMsecs();
    _RefreshState();
    switch (_curState)
    {
        case ubt_diagram_initial:
        {
            _EnterInitial();
            break;
        }
        case ubt_diagram_nrfault:
        {
            if (_curTask != NULL)
            {
                _curTask->Stop(ubt_task_result_sys_nrfault);
            }
            break;
        }
        case ubt_diagram_closed:
        {
            if (_curTask != NULL)
            {
                _curTask->Stop(ubt_task_result_sys_poweroff);
            }
            _SaveCfg();
            break;
        }
        default:
            ;
    }
    
}

void CStateDiagram::_EnterInitial()
{
    /*添加硬件检测代码
    * 检测成功则切换到wait/Initposes
    * 检测失败切换到可/不可恢复状态
    */
}

void CStateDiagram::_PublishState()
{
    int index = (int)_statePub;
    std_msgs::msg::String msg;
    msg.data = g_StatePubStrArr[index];
    _pub_state->publish(msg);

    std_msgs::msg::String msg2;
    ApiModStatus modStatus;
    modStatus.stamp = ubt_com::GetNowInMsecs();
    modStatus.state = msg.data;
    ApiCode *pApiCode = &modStatus.hw_UpCode;
    if ((modStatus.stamp - _curStatus.rackPoseTime) > 1000)
    {
        pApiCode->state = 0;
    }
    else
    {
        pApiCode->state = 1;
        pApiCode->tag = _curStatus.rackTag;
        pApiCode->x = _curStatus.rackPose.x;
        pApiCode->y = _curStatus.rackPose.y;
        pApiCode->theta = _curStatus.rackPose.theta;
    }
    modStatus.hw_LiftState = _curStatus.liftState;
    msg2.data = EncodeApiModStatus(modStatus);
    _pub_status->publish(msg2);

}

void CStateDiagram::_PublishInitialPose(geometry_msgs::msg::Pose2D _initpose)
{
    geometry_msgs::msg::PoseWithCovarianceStamped p;
    p.pose.pose.position.x = _initpose.x ;
    p.pose.pose.position.y = _initpose.y;

    geometry_msgs::msg::Quaternion qua = ubt_com::convertThetaToQuaternion(_initpose.theta);
    // p.pose.pose.orientation = tf::createQuaternionMsgFromYaw(_initpose.theta);
    p.pose.pose.orientation = qua;
    p.header.frame_id = "map";
    p.header.stamp = rclcpp::Clock().now();
    // Copy in the covariance, converting from 3-D to 6-D
    if (_cfg.loc_alg == ubt_locate_alg_amr)
    {
        p.pose.covariance[6 * 0 + 0] = 0.5 * 0.5;
        p.pose.covariance[6 * 1 + 1] = 0.5 * 0.5;
        p.pose.covariance[6 * 3 + 3] = 3.1415 / 12.0 * 3.1415 / 12.0;
    }else if(_cfg.loc_alg == ubt_locate_alg_mag)
    {
        p.pose.covariance[6 * 0 + 0] = 0.5 * 0.5;
        p.pose.covariance[6 * 1 + 1] = 0.5 * 0.5;
        p.pose.covariance[6 * 3 + 3] = 3.1415 / 12.0 * 3.1415 / 12.0;
    }
    _pub_initial->publish(p);

}

void CStateDiagram::_PublishRmctlMode()
{
    std_msgs::msg::UInt8 msg;
    msg.data = _rmctlMode;
    RCLCPP_INFO_STREAM(node->get_logger(),"msg: " << msg.data);
    _pub_rmctl_mode->publish(msg);
}
//发布避障警告
void CStateDiagram::_PublishAlarmForObstacle()
{
    char buf[64];
    long timdOff = ubt_com::GetNowInMsecs() - _statePubEnterTime;
    std_msgs::msg::String msg;
    sprintf(buf, "alarm obstacle %d %d", _obstacle_alarm_cnt, (int)(timdOff / 1000));
    msg.data = buf;
    RCLCPP_INFO(node->get_logger(),"msg= %s ", buf);
    _pub_obstacle_alarm->publish(msg);
}
//硬件自检
void CStateDiagram::_OnTimerInInitial()
{
    if(_is_initial)
    {
        _SetState(ubt_diagram_wait);
        return;
    }
    if(_agv_type == "t1000_v2")
    {
        _SetState(ubt_diagram_wait);

    }else if(_agv_type == "u1000_v3")
    {
        #ifdef UBT_LIFT_SUPPORT
        if (_curStatus.liftState == lift_state_unknown)
        {
            return;
        }
        //初始化顶升动作，先上升后下降
        while(rclcpp::ok() && _curStatus.liftState != lift_state_up )
        {
            std_msgs::msg::Float32 msg;
            msg.data = 0.008;
            _pub_lift_speed->publish(msg);
            rclcpp::spin_some(node);
        }
        while(rclcpp::ok() && _curStatus.liftState != lift_state_down )
        {
            std_msgs::msg::Float32 msg;
            msg.data = -0.008;
            _pub_lift_speed->publish(msg);
            rclcpp::spin_some(node);
        }
        #endif
        if (_lastState == ubt_diagram_closed) //上一次状态如果是关机状态，则需要进行初始定位
        {
            _SetState(ubt_diagram_initpose);//切换到初始定位状态
        }
        else  //上一次不是关机状态，切换到空闲状态
        {
            _SetState(ubt_diagram_wait);
        }
    }else if(_agv_type == "f1200")
    {
        #ifdef UBT_LIFT_SUPPORT
        if (_curStatus.liftState == lift_state_unknown)
        {
            return;
        }
        //初始化顶升动作，先上升后下降

        while(rclcpp::ok() && _curStatus.liftState != lift_state_up )
        {
            std_msgs::msg::Float32MultiArray liftTypeAndHeight;
            std_msgs::msg::Float32 msg;
            msg.data = 0.008;
            liftTypeAndHeight.data.push_back(msg.data);
            liftTypeAndHeight.data.push_back(90.0);

            _pub_lift_speed_and_distance->publish(liftTypeAndHeight);
            rclcpp::spin_some(node);
        }
        while(rclcpp::ok() && _curStatus.liftState != lift_state_down )
        {
            std_msgs::msg::Float32MultiArray liftTypeAndHeight;
            std_msgs::msg::Float32 msg;
            msg.data = -0.008;
            liftTypeAndHeight.data.push_back(msg.data);
            liftTypeAndHeight.data.push_back(0.0);

            _pub_lift_speed_and_distance->publish(liftTypeAndHeight);
            rclcpp::spin_some(node);
        }

        #endif
        if (_lastState == ubt_diagram_closed) //上一次状态如果是关机状态，则需要进行初始定位
        {
            _SetState(ubt_diagram_initpose);//切换到初始定位状态
        }
        else  //上一次不是关机状态，切换到空闲状态
        {
            _SetState(ubt_diagram_wait);
        }
    }
    _is_initial = true;

}
//初始定位
void CStateDiagram::_OnTimerInInitPose()
{
    
    int i, iSel = -1;
    long startTm, callBeginTm, now;
#ifdef UBT_ALGS_QRCODE_SUPPORT
    if (_cfg.loc_alg == ubt_locate_alg_qrcode)
    {
        _SetPoseState(pose_state_locating);
        _SetState(ubt_diagram_wait);
        return;
    }
#endif
    if (_initPoses.size() == 0)
    {
        RCLCPP_INFO(node->get_logger(),"select pose none");
        std_msgs::msg::String msg;
        msg.data = "NG";
        _pub_inital_state->publish(msg);
        _SetState(ubt_diagram_wait);
        return;
    }
    if (_initPoseBelCnt == _initPoses.size())
    {
        return;
    }
    if (_curStatus.poseState != pose_state_waitloc)
    {
        return;
    }
    startTm = ubt_com::GetNowInMsecs();
    for (i = _initPoseBelCnt; i < _initPoses.size(); i++)
    {
        bool bRes;
        float bel;
        geometry_msgs::msg::Pose2D  poseOpt;
        callBeginTm = ubt_com::GetNowInMsecs();

        switch (_cfg.loc_alg)
        {
#ifdef UBT_USLAM_SUPPORT
            case ubt_locate_alg_uslam:
            {
                relocalization_module_pkg::EstimateRelocalization::Request req;
                relocalization_module_pkg::EstimateRelocalization::Response rsp;
                geometry_msgs::msg::PoseWithCovarianceStamped *p = &req.initial_pose;
                p->pose.pose.position.x = _initPoses[i].pose.x;
                p->pose.pose.position.y = _initPoses[i].pose.y;
                p->pose.pose.orientation = tf::createQuaternionMsgFromYaw(_initPoses[i].pose.theta);
                p->header.frame_id = "map";
                p->header.stamp = rclcpp::Clock().now();
                req.large_scale_flag = false;
                bRes = _cli_initnode_belpose.call(req, rsp);
                if (bRes)
                {
                    bel = rsp.estimate_score;
                    poseOpt.x = rsp.optimized_pose.pose.pose.position.x;
                    poseOpt.y = rsp.optimized_pose.pose.pose.position.y;
                    tf::Quaternion quat(0, 0, rsp.optimized_pose.pose.pose.orientation.z, rsp.optimized_pose.pose.pose.orientation.w);
                    poseOpt.theta = tf::getYaw(quat);
                }
                break;
            }
#endif
            default:
            {
                // ubt_initpose::bel_pose::Request req;
                // ubt_initpose::bel_pose::Response rsp;
                auto request = std::make_shared<ubt_interface::srv::BelPose::Request>();

                request->x = _initPoses[i].pose.x;
                request->y = _initPoses[i].pose.y;
                request->theta = _initPoses[i].pose.theta;

                while (!_cli_initnode_belpose->wait_for_service(1s)) { 
                    RCLCPP_INFO(node->get_logger(),"/bel_pose service not available, waiting again...");
                }

                auto result = _cli_initnode_belpose->async_send_request(request); 
                if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)  
                {
                    bRes = true;
                    RCLCPP_INFO(node->get_logger(),"请求正常处理,响应结果: is success: %d, bel: %f",true, result.get()->bel);
                }
                else
                {
                    bRes = false;
                    RCLCPP_ERROR(node->get_logger(),"请求处理失败....");
                    return;
                }
                // bRes = _cli_initnode_belpose.call(req, rsp);
                if (bRes)
                {
                    bel = result.get()->bel;
                    poseOpt.x = _initPoses[i].pose.x;
                    poseOpt.y = _initPoses[i].pose.y;
                    poseOpt.theta = _initPoses[i].pose.theta;
                }
            }
        }
        now = ubt_com::GetNowInMsecs();
        if (!bRes)
        {
            RCLCPP_WARN(node->get_logger(),"call service belpose(%lf,%lf,%lf)  fail, cost=%ld name=%s",
                     _initPoses[i].pose.x, _initPoses[i].pose.y, _initPoses[i].pose.theta, (now - callBeginTm), _initPoses[i].name.c_str());
            return;
        }
            RCLCPP_INFO(node->get_logger(),"call service belpose(%lf,%lf,%lf) ok,cost= %ld name=%s bel=%lf ",
                 _initPoses[i].pose.x, _initPoses[i].pose.y, _initPoses[i].pose.theta, (now - callBeginTm), _initPoses[i].name.c_str(), bel);
        _initPoseBelCnt++;
        if (bel < _initPoseBel)
        {
            continue;
        }
        _initPoseSel = i;
        _initPoseBel = bel;
        _initPose = poseOpt;
        if ((now - startTm) >= 50)
        {
            break;
        }
    }
    if (_initPoseBelCnt == _initPoses.size())
    {
        RCLCPP_INFO(node->get_logger(),"select pose ,name=%s x=%lf y=%lf theta=%lf bel=%lf iSel=%d ",
                 _initPoses[_initPoseSel].name.c_str(), _initPose.x, _initPose.y, _initPose.theta, _initPoseBel, _initPoseSel);
        _PublishInitialPose(_initPose);
    }
    
}


void CStateDiagram::_CheckObstacleStatus()
{
    
    StateStatus *pStatus = &_curStatus;
    int avoidHwStatus, laserAvoidStatus = pStatus->laserAvoidStatus;
    if ((laserAvoidStatus == avoid_status_value_stopleft) ||
            (laserAvoidStatus == avoid_status_value_stopright))
    {
        laserAvoidStatus = avoid_status_value_slow;
    }
    if (laserAvoidStatus == 0)
    {
        avoidHwStatus = pStatus->rgbdAvoidStatus ;
    }
    else if (pStatus->rgbdAvoidStatus == 0)
    {
        avoidHwStatus = laserAvoidStatus ;
    }
    else if (laserAvoidStatus < pStatus->rgbdAvoidStatus)
    {
        avoidHwStatus = laserAvoidStatus;
    }
    else
    {
        avoidHwStatus = pStatus->rgbdAvoidStatus;
    }
    if (avoidHwStatus != pStatus->avoidHwStatus)
    {
        pStatus->avoidHwStatus = avoidHwStatus;
        if (_curTask != NULL)
        {
            _curTask->OnMsg(ubt_msg_type_avoid_status, &avoidHwStatus);
        }
        else
        {
            _RefreshState();
        }
    }
    
}
//检查本体膨胀信息
void CStateDiagram::_CheckExpInfo(int type)
{
    float expLength, expWidth, rackTheta = _curStatus.rackPose.theta;
    if (_curStatus.liftState == lift_state_up)//顶升
    {
        if (fabs(ubt_com::getRegularTheta(rackTheta - 0)) < 0.05)
        {
            expLength = _cfg.rack_length;
            expWidth = _cfg.rack_width;
            _curStatus.expTheta = 0;
        }
        else if (fabs(ubt_com::getRegularTheta(rackTheta + 1.57)) < 0.05)
        {
            expLength = _cfg.rack_width;
            expWidth = _cfg.rack_length;
            _curStatus.expTheta = -1.57;
        }
        else if (fabs(ubt_com::getRegularTheta(rackTheta - 3.14)) < 0.05)
        {
            expLength = _cfg.rack_length;
            expWidth = _cfg.rack_width;
            _curStatus.expTheta = 3.14;
        }
        else if (fabs(ubt_com::getRegularTheta(rackTheta - 1.57)) < 0.05)
        {
            expLength = _cfg.rack_width;
            expWidth = _cfg.rack_length;
            _curStatus.expTheta = 1.57;
        }
        else
        {
            if (type == 1)
            {
                expLength = sqrt(_cfg.rack_width * _cfg.rack_width + _cfg.rack_length * _cfg.rack_length);
                expWidth =  expLength;
            }
            else
            {
                return;
            }
        }
    }
    else if(_curStatus.dragState == drag_state_up)//牵引销升起
    {
        expLength = _cfg.rack_length;
        expWidth = _cfg.rack_width;
    }
    else if(_curStatus.dragState == drag_state_down)//放下时，发默认的避障长度距离
    {
        expLength = _cfg.rack_dis;
        expWidth = _cfg.robot_width;
    }
    else
    {
        expLength = _cfg.robot_length;
        expWidth = _cfg.robot_width;
    }
    if (expWidth == _curStatus.expWidth && expLength == _curStatus.expLength)
    {
        return;
    }
    _curStatus.expLength = expLength;
    _curStatus.expWidth = expWidth;

    std_msgs::msg::Float32MultiArray msg2;
    msg2.data.push_back(1);
    msg2.data.push_back(_curStatus.expLength);
    msg2.data.push_back(_curStatus.expWidth);
    double rotateRadius = std::sqrt(std::pow(_cfg.robot_length, 2) + std::pow(_cfg.robot_width, 2)) / 2;
    if (_curStatus.liftState == lift_state_up)
    {
        msg2.data.push_back(_cfg.rack_legLength);
        msg2.data.push_back(_cfg.rack_legWidth);

        // 判断是二维码还是slam版本，二维码版本的旋转避障半径直接使用机器人的半径，不需要改变
        // slam版本顶起货架后，旋转避障半径需要使用带货架后的半径
        if(ubt_locate_alg_amr == _cfg.loc_alg)
        {
            rotateRadius = sqrt(_cfg.rack_width * _cfg.rack_width + _cfg.rack_length * _cfg.rack_length) / 2;
        }else if(ubt_locate_alg_mag == _cfg.loc_alg)
        {
            rotateRadius = sqrt(_cfg.rack_width * _cfg.rack_width + _cfg.rack_length * _cfg.rack_length) / 2;
        }

        #ifdef UBT_USLAM_SUPPORT
        if(ubt_locate_alg_uslam == _cfg.loc_alg)
        {
            rotateRadius = sqrt(_cfg.rack_width * _cfg.rack_width + _cfg.rack_length * _cfg.rack_length) / 2;
        }
        #endif
    }
    else
    {
        msg2.data.push_back(0);
        msg2.data.push_back(0);
    }

    msg2.data.push_back(rotateRadius);

    RCLCPP_INFO(node->get_logger(),"publish expLength=%f expWidth=%f, "
        "rotateRadius=%f", expLength, expWidth, rotateRadius);
    _pub_obstacle_exp->publish(msg2);
}

void CStateDiagram::_SetObsParams(int type)
{
    ObstacleParams obsParams;
    obsParams = _cfg.obsParams;
    if (type == 0)
    {
        obsParams.stopDis = _cfg.obstacle_rmctl_dis;
    }
    else
    {
        obsParams.stopDis = _cfg.obstacle_mapping_dis;
    }
    UbtStateUtilSetObstacleParams(obsParams);
}

void CStateDiagram::_SetPoseState(int poseState)
{
    _curStatus.poseState = poseState;
    _curStatus.poseStateTime = ubt_com::GetNowInMsecs();
    if (poseState != pose_state_ok)
    {
        _poseChecker.state = 0;
    }
}

void CStateDiagram::_SetStopDis(const FixePathTaskInfo& taskInfo)
{
    // double stop_distance = taskInfo.path[0].points[0].avoidDis;
    // double next_point = taskInfo.path[0].points[1].avoidDis;
    // RCLCPP_INFO(node->get_logger(),"SET stopdis function working, get stop distance from web :%.2f next point is :%.2f", stop_distance, next_point);

    if (_agv_type.substr(0, 5) == "t1000" || _agv_type.substr(0, 5) == "u1000" || _agv_type.substr(0, 4) == "u600")
    {
        ObstacleParams obsParams;
        obsParams.stopDis = _cfg.obstacle_run_start_up_dis;  // 起步避障距离
        obsParams.slowDis = _cfg.obsParams.slowDis;
        obsParams.warnDis = _cfg.obsParams.warnDis;
        obsParams.sideDis = _cfg.obsParams.sideDis;
        UbtStateUtilSetObstacleParams(obsParams);
    }
    else
    {
        // ros::service::waitForService("/aeb/set_stop_distance");
        // aeb::SetStopDistance ssd_srv;

        // ssd_srv.request.distance = stop_distance;

        // bool flag = _set_stop_distance_client.call(ssd_srv);
        // if (flag)
        // {
        //     RCLCPP_INFO(node->get_logger(),"请求正常处理,响应结果: is success: %d, msg: %s",ssd_srv.response.success, ssd_srv.response.message.c_str());
        // }
        // else
        // {
        //     RCLCPP_ERROR(node->get_logger(),"请求处理失败....");
        //     return;
        // }
    }
}



void CStateDiagram::_SetRackInfo(const FixePathTaskInfo& taskInfo)
{
    double length = taskInfo.mapGood.length;
    double width = taskInfo.mapGood.width;
    double legLength = taskInfo.mapGood.legLength;
    double legWidth = taskInfo.mapGood.legWidth;
    //if have value from web
    if(length > 1e-6)  _cfg.rack_length = length;
    if(width > 1e-6)   _cfg.rack_width = width;
    if(legLength > 1e-6)   _cfg.rack_legLength = legLength;
    if(legWidth > 1e-6)    _cfg.rack_legWidth = legWidth;

    if (_agv_type == "u1000_v3" || _agv_type == "t1000_v2" || _agv_type == "u600")
    {
        return ;
    }

    RCLCPP_INFO(node->get_logger(),"SET rackinfo function working");
    // ros::service::waitForService("/aeb/set_footprint");
    while (!_set_footprint_client->wait_for_service(1s)) { 
        RCLCPP_INFO(node->get_logger(),"/aeb/set_footprint service not available, waiting again...");
    }
    // aeb::SetFootprint sft_srv;
    auto request = std::make_shared<ubt_interface::srv::SetFootprint::Request>();

    // 初始化xmlrpc环境
    xmlrpc_value *footprint_params;
    xmlrpc_env env;
    
    
    geometry_msgs::msg::Point32 pt;
    // if (_nh.getParam("/aeb/footprint", footprint_params)) 
    std::string footprint_str = node->get_parameter("/aeb/footprint").as_string();
     // 创建xmlrpc_value
    footprint_params = xmlrpc_string_new(&env, footprint_str.c_str());
    if (env.fault_occurred){
        // 处理错误
        std::cerr << "转换失败: " << env.fault_string << std::endl;
        xmlrpc_env_clean(&env);
        return ;
    }
 

    
    if (footprint_params)
    {
        RCLCPP_INFO(node->get_logger(),"get footprint param from yaml file");
        int size = xmlrpc_array_size(&env,footprint_params);
        if ( size != 4)
        {
            RCLCPP_INFO(node->get_logger(),"footprint param size is not 4");
            return;
        }

        
        xmlrpc_double num0;
        xmlrpc_double num1;
        xmlrpc_value *value;
        xmlrpc_value *num0_value;
        xmlrpc_value *num1_value;

        for (size_t i = 0; i < size; ++i) {

            xmlrpc_array_read_item(&env,footprint_params,i,&value);
            xmlrpc_array_read_item(&env,value,0,&num0_value);
            xmlrpc_array_read_item(&env,value,1,&num1_value);
            xmlrpc_read_double(&env,num0_value,&num0);
            xmlrpc_read_double(&env,num1_value,&num1);

            const double &x = num0;
            const double &y = num1;
            RCLCPP_INFO_STREAM(node->get_logger(),"footprint " << i << " [" << x << " "<< y << "]");
        }
  
        for (size_t i = 0; i < size; ++i) 
        {
            xmlrpc_array_read_item(&env,footprint_params,i,&value);
            xmlrpc_array_read_item(&env,value,0,&num0_value);
            xmlrpc_array_read_item(&env,value,1,&num1_value);
            xmlrpc_read_double(&env,num0_value,&num0);
            xmlrpc_read_double(&env,num1_value,&num1);

            double index = -1.0;
            if(i == 0 || i == 3)
            {
                index = 1.0;
            }
            else
            {
                index = -1.0;
            }
            const double &x = num0;
            const double &y = index * width;
            pt.x = x;
            if (y == 0)
            {
        	    pt.y = static_cast<float>(static_cast<double>(num1));
            }
            else
            {
                pt.y = y / 2.0;
            }
            RCLCPP_INFO(node->get_logger(),"ready to call set footprint service, value ready y = %.2f ", width);
            request->footprint.points.push_back(pt);
        }
    }
        // 清理环境和xmlrpc_value
    xmlrpc_DECREF(footprint_params);
    xmlrpc_env_clean(&env);

    if (taskInfo.mapGood.width != 0)
    {
        // bool flag = _set_footprint_client.call(sft_srv);
        auto result = _set_footprint_client->async_send_request(request); 
        if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)  
        {
   
            RCLCPP_INFO(node->get_logger(),"drag_state_up 请求正常处理,响应结果: is success: %d, msg: %s",result.get()->success, result.get()->message.c_str());
        }
        else
        {

            RCLCPP_ERROR(node->get_logger(),"请求处理失败....");
            return;
        }
        // if (flag)
        // {
        //     std::cout << sft_srv.response.message << std::endl;
        //     RCLCPP_INFO(node->get_logger(),"drag_state_up 请求正常处理,响应结果: is success: %d, msg: %s",sft_srv.response.success, sft_srv.response.message.c_str());
        // }
        // else
        // {
        //     RCLCPP_ERROR(node->get_logger(),"请求处理失败....");
        //     return;
        // }
    }

    if (taskInfo.mapGood.width == 0 && _curStatus.dragState == drag_state_down)
    {
        auto result = _set_footprint_client->async_send_request(request); 
        if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)  
        {

            RCLCPP_INFO(node->get_logger(),"drag_state_down 请求正常处理,响应结果: is success: %d, msg: %s",result.get()->success, result.get()->message.c_str());

        }
        else
        {
    
            RCLCPP_ERROR(node->get_logger(),"请求处理失败....");
            return;
        }
        // bool flag = _set_footprint_client.call(sft_srv);
        // if (flag)
        // {
        //     RCLCPP_INFO(node->get_logger(),"drag_state_down 请求正常处理,响应结果: is success: %d, msg: %s",sft_srv.response.success, sft_srv.response.message.c_str());
        // }
        // else
        // {
        //     RCLCPP_ERROR(node->get_logger(),"请求处理失败....");
        //     return;
        // }
    }
    

}

void CStateDiagram::_PoseCheckerTimeout()
{
    do
    {
        if (_curStatus.poseState != pose_state_ok)
        {
            break;
        }
        if (_poseChecker.state == 0)
        {
            if ((_curStatus.curPoseTime < _curStatus.poseStateTime) ||
                    (_curStatus.odomPoseTime < _curStatus.poseStateTime))
            {
                break;
            }
            _poseChecker.state = 1;
            _poseChecker.prevPose = _curStatus.curPose;
            _poseChecker.prevOdomPose = _curStatus.odomPose;
            _poseChecker.disMax = 0.0;
            break;
        }
        double xOff = _curStatus.curPose.x - _poseChecker.prevPose.x;
        double yOff = _curStatus.curPose.y - _poseChecker.prevPose.y;
        double dis = sqrt(xOff * xOff + yOff * yOff);
        double odomXOff = _curStatus.odomPose.x - _poseChecker.prevOdomPose.x;
        double odomYOff = _curStatus.odomPose.y - _poseChecker.prevOdomPose.y;
        double odomDis = sqrt(odomXOff * odomXOff + odomYOff * odomYOff);
        double disBig, ratio;
        if (dis >= odomDis)
        {
            ratio = odomDis / dis;
            disBig = dis;
        }
        else
        {
            ratio = dis / odomDis;
            disBig = odomDis;
        }
        if (disBig < _poseChecker.disMax)
        {
            if ((disBig + _cfg.pose_check_dis * 0.2) < _poseChecker.disMax)
            {
                RCLCPP_INFO(node->get_logger(),"check ignore,ratio= %lf disBig=%lf disMax=%lf mrpt= %f (%f,%f,%f)->(%f,%f,%f) odom= %f (%f,%f,%f)->(%f,%f,%f)",
                         ratio, disBig, _poseChecker.disMax,
                         dis, _poseChecker.prevPose.x, _poseChecker.prevPose.y, _poseChecker.prevPose.theta,
                         _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta,
                         odomDis, _poseChecker.prevOdomPose.x, _poseChecker.prevOdomPose.y, _poseChecker.prevOdomPose.theta,
                         _curStatus.odomPose.x, _curStatus.odomPose.y, _curStatus.odomPose.theta
                        );
                _poseChecker.prevPose = _curStatus.curPose;
                _poseChecker.prevOdomPose = _curStatus.odomPose;
                _poseChecker.disMax = 0.0;
            }
            break;
        }
        _poseChecker.disMax = disBig;
        if ((dis < _cfg.pose_check_dis) && (odomDis < _cfg.pose_check_dis))
        {
            break;
        }
        if (ratio < _cfg.pose_check_ratio)
        {
            RCLCPP_WARN(node->get_logger(),"check fail,ratio= %lf mrpt= %f (%f,%f,%f)->(%f,%f,%f) odom= %f (%f,%f,%f)->(%f,%f,%f)",
                     ratio,
                     dis, _poseChecker.prevPose.x, _poseChecker.prevPose.y, _poseChecker.prevPose.theta,
                     _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta,
                     odomDis, _poseChecker.prevOdomPose.x, _poseChecker.prevOdomPose.y, _poseChecker.prevOdomPose.theta,
                     _curStatus.odomPose.x, _curStatus.odomPose.y, _curStatus.odomPose.theta
                    );
            _SetPoseState(pose_state_waitloc);
            if (_curTask != NULL)
            {
                _curTask->Stop(ubt_task_result_sys_posefail);
            }
            break;
        }
            RCLCPP_INFO(node->get_logger(),"check ok,ratio= %lf mrpt= %f (%f,%f,%f)->(%f,%f,%f) odom= %f (%f,%f,%f)->(%f,%f,%f)",
                 ratio,
                 dis, _poseChecker.prevPose.x, _poseChecker.prevPose.y, _poseChecker.prevPose.theta,
                 _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta,
                 odomDis, _poseChecker.prevOdomPose.x, _poseChecker.prevOdomPose.y, _poseChecker.prevOdomPose.theta,
                 _curStatus.odomPose.x, _curStatus.odomPose.y, _curStatus.odomPose.theta
                );
        _poseChecker.prevPose = _curStatus.curPose;
        _poseChecker.prevOdomPose = _curStatus.odomPose;
        _poseChecker.disMax = 0.0;
    }
    while (false);
}

void CStateDiagram::_NotifyEvent(int evType, std::string evMsg)
{
    if (evType == 1)
    {
        std_msgs::msg::String msg;
        msg.data = "reset";
        _pub_reset_dispatch->publish(msg);
    }
}

void CStateDiagram::_NotifyTaskStatus(std::string taskId, std::string status, int actionIndex, std::string actionType, std::string desc)
{
    ApiTaskStatus taskStatus;
    taskStatus.taskId = taskId;
    taskStatus.status = status;
    taskStatus.actionIndex = actionIndex;
    taskStatus.actionType = actionType;
    taskStatus.desc = desc;
    std_msgs::msg::String msg;
    msg.data = EncodeTaskStatus(taskStatus);
    _pub_task_status->publish(msg);
}


static int _ParseStatePoseEx(std::string poseExStr, StatePoseEx& poseEx)
{
    std::map<std::string, std::string> paramMap = ubt_com::ParseUrlQuery(poseExStr);
    if (paramMap.find("x") == paramMap.end())
    {
        return -1;
    }
    poseEx.pose.x = atof(paramMap["x"].c_str());
    poseEx.pose.y = atof(paramMap["y"].c_str());
    poseEx.pose.theta = atof(paramMap["theta"].c_str());
    poseEx.name = paramMap["name"];
    poseEx.type = paramMap["type"];
    return 0;
}

bool CStateDiagram::_LoadCfg()
{
    node->declare_parameter<std::string>("/system/state_save_file", "");
    _cfg.state_save_file = node->get_parameter("/system/state_save_file").as_string();
    if (_cfg.state_save_file == "")
    {
        RCLCPP_WARN(node->get_logger(),"_LoadCfg fail, param state_save_file should be set before start!!!");
        return -1;
    }
        
    node->declare_parameter<int>("obstacle_alarm_duration", 1800);
    _cfg.obstacle_alarm_duration = node->get_parameter("obstacle_alarm_duration").as_int();
    node->declare_parameter<double>("obstacle_stop_dis", 0.6);
    _cfg.obsParams.stopDis = node->get_parameter("obstacle_stop_dis").as_double();
    node->declare_parameter<double>("obstacle_slow_dis", 0.8);
    _cfg.obsParams.slowDis = node->get_parameter("obstacle_slow_dis").as_double();
    node->declare_parameter<double>("obstacle_warn_dis", 1.0);
    _cfg.obsParams.warnDis = node->get_parameter("obstacle_warn_dis").as_double();
    node->declare_parameter<double>("obstacle_side_dis", 0.15);
    _cfg.obsParams.sideDis = node->get_parameter("obstacle_side_dis").as_double();
    node->declare_parameter<double>("obstacle_run_start_up_dis", 0.15);
    _cfg.obstacle_run_start_up_dis = node->get_parameter("obstacle_run_start_up_dis").as_double();
    node->declare_parameter<double>("obstacle_mapping_dis", 0.1);
    _cfg.obstacle_mapping_dis = node->get_parameter("obstacle_mapping_dis").as_double();
    node->declare_parameter<double>("obstacle_rmctl_dis", 0.3);
    _cfg.obstacle_rmctl_dis = node->get_parameter("obstacle_rmctl_dis").as_double();
    node->declare_parameter<double>("obstacle_auto_find_dis", 0.3);
    _cfg.obstacle_auto_find_dis = node->get_parameter("obstacle_auto_find_dis").as_double();

    node->declare_parameter<double>("pose_fail_bel_threshold", 0.0);
    _cfg.pose_fail_bel_threshold = node->get_parameter("pose_fail_bel_threshold").as_double();

    node->declare_parameter<std::string>("/system/last_pose", "");
    _cfg.last_pose = node->get_parameter("/system/last_pose").as_string();

    node->declare_parameter<std::string>("/system/init_poses", "");
    _cfg.init_poses = node->get_parameter("/system/init_poses").as_string();
    StatePoseEx poseExLast, poseEx;
    if (_ParseStatePoseEx(_cfg.last_pose, poseExLast) == 0)
    {
        //init_pose不再使用
        // std::vector<std::string> strs = ubt_com::SpitStr(_cfg.init_poses, ';');
        // for (std::vector<std::string>::iterator iter = strs.begin(); iter != strs.end(); ++iter)
        // {
        //     if (_ParseStatePoseEx(*iter, poseEx) == 0)
        //     {
        //         if ((fabs(poseEx.pose.x - poseExLast.pose.x) > 1) ||
        //                 (fabs(poseEx.pose.y - poseExLast.pose.y) > 1))
        //         {
        //             continue;
        //         }
        //         _initPoses.push_back(poseEx);
        //     }
        // }
    }
    std::string  locAlg;
    node->declare_parameter<std::string>("agv_loc_algs", "");
    locAlg = node->get_parameter("agv_loc_algs").as_string();
    if (locAlg == "amr")
    {
        _cfg.loc_alg = ubt_locate_alg_amr;
        node->declare_parameter<double>("first_locate_bel_threshold_amr", 0.0);
        _cfg.first_locate_bel_threshold = node->get_parameter("first_locate_bel_threshold_amr").as_double();
    }
    else if (locAlg == "mag")
    {
        _cfg.loc_alg = ubt_locate_alg_mag;
    }
#ifdef UBT_USLAM_SUPPORT
    else if (locAlg == "uslam")
    {
        _cfg.loc_alg = ubt_locate_alg_uslam;
        node->declare_parameter<double>("first_locate_bel_threshold_uslam", 0.5);
        _cfg.first_locate_bel_threshold = node->get_parameter("first_locate_bel_threshold_uslam").as_double();
    }
#endif
#ifdef UBT_ALGS_QRCODE_SUPPORT
    else if (locAlg == "qrcode")
    {
        _cfg.loc_alg = ubt_locate_alg_qrcode;
    }
#endif
    else
    {
        RCLCPP_WARN(node->get_logger(),"_LoadCfg fail, param agv_loc_algs should be amr , uslam or qrcode ;but is %s !!!", locAlg.c_str());
        return -1;
    }
    node->declare_parameter<double>("pose_check_dis", 2.0);
    _cfg.pose_check_dis = node->get_parameter("pose_check_dis").as_double();
    node->declare_parameter<double>("pose_check_ratio", 0.95);
    _cfg.pose_check_ratio = node->get_parameter("pose_check_ratio").as_double();

#if (PLAT_USE == PLAT_U1000)

    node->declare_parameter<double>("/hw_robot_width", 0.837);
    _cfg.robot_width = node->get_parameter("/hw_robot_width").as_double();
    node->declare_parameter<double>("/hw_robot_length", 1.101);
    _cfg.robot_length = node->get_parameter("/hw_robot_length").as_double();
#else
    node->declare_parameter<double>("/hw_robot_width", 0.598);
    _cfg.robot_width = node->get_parameter("/hw_robot_width").as_double();
    node->declare_parameter<double>("/hw_robot_length", 0.8);
    _cfg.robot_length = node->get_parameter("/hw_robot_length").as_double();
#endif

    _LoadDyamicCfg();
    
    return 0;
}

void CStateDiagram::_LoadDyamicCfg()
{
    node->declare_parameter<double>("rack_width", DEFAULT_GOOD_WIDTH);
    _cfg.rack_width = node->get_parameter("rack_width").as_double();
    node->declare_parameter<double>("rack_length", DEFAULT_GOOD_LENGTH);
    _cfg.rack_length = node->get_parameter("rack_length").as_double();
    node->declare_parameter<double>("rack_legLength", DEFAULT_GOOD_LEG_LENGTH);
    _cfg.rack_legLength = node->get_parameter("rack_legLength").as_double();
    node->declare_parameter<double>("rack_legWidth", DEFAULT_GOOD_LEG_WIDTH);
    _cfg.rack_legWidth = node->get_parameter("rack_legWidth").as_double();
    node->declare_parameter<double>("rack_dis", DEFAULT_GOOD_LEG_LENGTH);
    _cfg.rack_dis = node->get_parameter("rack_dis").as_double();
    node->declare_parameter<int>("nav2_check_timeout", 1);
    _cfg.nav2_check_timeout = node->get_parameter("nav2_check_timeout").as_int();

    _curStatus.nav2CheckTimeout = _cfg.nav2_check_timeout;
}

int CStateDiagram::_SaveCfg()
{
    
    int fd, len, nBytes, errNum;
    char buf[1024];
    std::string tmpFname = _cfg.state_save_file + ".tmp";
    if ((fd = open(tmpFname.c_str(), O_CREAT | O_TRUNC | O_RDWR, S_IRUSR  | S_IWUSR | S_IRGRP)) < 0)
    {
        RCLCPP_WARN(node->get_logger(),"open fail errno=%d fname=%s",
                 errno, tmpFname.c_str());
        return -1;
    }
    len = sprintf(buf, "/system/last_pose: x=%lf&y=%lf&theta=%lf&name=last&type=last\n",
                  _curStatus.curPose.x, _curStatus.curPose.y, _curStatus.curPose.theta);
    nBytes = write(fd, buf, len);
    close(fd);
    if (nBytes != len)
    {
        RCLCPP_WARN(node->get_logger()," write fail len=%d nBytes=%d errno=%d fname=%s",
                 len, nBytes, errno, tmpFname.c_str());
        return -2;
    }
    if (0 != rename(tmpFname.c_str(), _cfg.state_save_file.c_str()))
    {
        RCLCPP_WARN(node->get_logger()," rename fail errno=%d fname=%s",
                 errno, tmpFname.c_str());
        return -2;
    }
    RCLCPP_INFO(node->get_logger(),"ok ,len=%d fname=%s",
             len, _cfg.state_save_file.c_str());
    
    return 0;
}

void CStateDiagram::_visualFinePositioning()
{
    if (_curTask != NULL && _statePub != ubt_Wait && _statePub != ubt_WaitPose && _statePub != ubt_Close)
    {
        RCLCPP_ERROR(node->get_logger(),"[CStateDiagram] _visualFinePositioning : Current task is not null and not free, return!");
        return ;
    }
    geometry_msgs::msg::Pose2D poseDst;  //目标位姿
    poseDst.x = 0.0;
    poseDst.y = 0.0;
    poseDst.theta = ubt_proto_adjust::ubt_adjust_theta_4x90;

    ubt_proto_adjust::ubt_adjust_pose_type poseType;  //目标位姿类型
    geometry_msgs::msg::Pose2D  poseLmt;//允许的偏差最大值
    poseLmt.x = 0.01;
    poseLmt.y = 0.01;
    poseLmt.theta = 0.01;

    float stopDis = 0.0;
    float sideDis = 0.0;
    int avoidHwMask = 2;
    int taskIndex = 0;
    int reportIndex = 0;
    std::string actionType = "visual_fine_positioning";

    _curTask = CTaskAdjust::CreateTask(this, "", ubt_task_stage_action, 120000,
        ubt_proto_adjust::ubt_adjust_pose_qrcode, poseDst, poseLmt, stopDis, sideDis, avoidHwMask,
        taskIndex, reportIndex, actionType);

    _curTask->Start();

    // adjustTask->Start();
    // delete adjustTask;

    // while (_qrCodePose.x < 1e-6 && _qrCodePose.y < 1e-6 && _qrCodePose.theta < 1e-6)
    // {
    //     // 计算到目标点的距离和角度
    //     double distance = sqrt(pow(_qrCodePose.x, 2) + pow(_qrCodePose.y, 2));
    //     double angle = atan2(_qrCodePose.y, _qrCodePose.x);

    //     geometry_msgs::msg::Twist cmd_vel;
    //     cmd_vel.linear.x = 0.5 * distance;
    //     cmd_vel.angular.z = 0.3 * angle;

    //     // 发布速度控制命令
    //     cmd_vel_pub->publish(cmd_vel);

    // }
}

void CStateDiagram::_autoLinePatrolPause(bool isPause)
{
    if (_curStatus.autoFindState || _curStatus.isStandaloneMode)
    {
        if (isPause)
        {
            this->_autoFindPauseFlag = true;
            if (_curStatus.isStandaloneMode)
            {
                std_msgs::msg::Bool msgStandaloneMode;
                msgStandaloneMode.data = false;
                _pub_standalone_mode->publish(msgStandaloneMode);
            }
            else
            {
                std_msgs::msg::String msgInit;
                msgInit.data = "0";
                _pub_init_find_rfid->publish(msgInit);
            }
        }
        else
        {
            this->_autoFindPauseFlag = false;
            if (_curStatus.isStandaloneMode)
            {
                std_msgs::msg::Bool msgStandaloneMode;
                msgStandaloneMode.data = true;
                _pub_standalone_mode->publish(msgStandaloneMode);
            }
            else
            {
                std_msgs::msg::String msgInit;
                msgInit.data = "1";
                _pub_init_find_rfid->publish(msgInit);
            }
        }
    }
}

}




